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Abstract 

<N 

In this paper, we present an iterative steering algorithm for nonholonomic systems (also called 
driftless control-amne systems) and we prove its global convergence under the sole assumption 
that the Lie Algebraic Rank Condition (LARC) holds true everywhere. That algorithm is an 
extension of the one introduced in [21] for regular systems. The first novelty here consists in the 
explicit algebraic construction, starting from the original control system, of a lifted control system 
which is regular. The second contribution of the paper is an exact motion planning method for 
nilpotent systems, which makes use of sinusoidal control laws and which is a generalization of the 
algorithm described in [29] for chained-form systems. 
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1 Introduction 

Nonholonomic systems have been attracting the attention of the scientific community for several years, 
due to the theoretical challenges they offer and the numerous important applications they cover. From 
the point of view of control theory, a nonholonomic system is a driftless control-affine system and is 
written as 

m 

(E) x = ^2uiXi(x), x G Q, u = (ui, . . . , u m ) G M m , (1.1) 

i=l 

where Q is an open connected subset of M n , and Xi, . . . ,X m are C°° vector fields on Q. Admissible 
inputs are M m - valued measurable functions u(-) defined on some interval [0,T] and a trajectory of (£), 
corresponding to some Xq G Q and to an admissible input u(-), is the (maximal) solution x(-) in Q of 
the Cauchy problem defined by x(t) = Y1T=\ u i(t)Xi(x(t)), t G [0,T], and x(0) = xq. 

In this paper, we address the motion planning problem (MPP for short) for (£), namely determine 
a procedure which associates with every pair of points (p, q) G VL x Q an admissible input «(•) defined 
on some interval [0, T], such that the corresponding trajectory of (E) starting from p at t — reaches q 
at t = T. As for the existence of a solution to MPP, this is equivalent to the complete controllability of 
(E). After the works of Chow and Rashevsky in the thirties [131 [32], and that of Sussmann and Stefan 
in the seventies [36, 35J, the issue of complete controllability for nonholomic systems is well-understood 
and it is usually guaranteed by assuming that the Lie Algebraic Rank Condition (also known as the 
Hormander condition) holds for (E). This easily checkable condition is not only sufficient for complete 
controllability but also necessary when the vector fields are analytic. From a practical viewpoint, 
assuming the LARC is, in a sense, the minimal requirement to ensure complete controllability for (E) 
and this is what we will do for all the control systems considered hereafter. 

As for the construction of the solutions of the MPP, we present, in this paper, a complete procedure 
solving the MPP for a nonholonomic system subject to the sole LARC. By "complete procedure" , we 
mean that the following properties must be guaranteed by the proposed procedure. 

1. Global character of the algorithm: for every pair of points (p, q) in Q, the algorithm must produce 
a steering control. (Note that the core of many algorithms consists in a local procedure and 
turning the latter into a global one is not always a trivial issue.) 

2. Proof of convergence of the algorithm. 

3. Regarding numerical implementations, no prohibitive limitation on the state dimension n. 
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4. Usefulness for practical applications, e.g., robustness with respect to the dynamics, "nice" trajec- 
tories produced by the algorithm, (no cusps neither large oscillations), and possibility of localizing 
the algorithm in order to handle obstacles (i.e., reducing the working space Q to any smaller open 
and connected subset of R ra ). 

There exist of course several algorithms addressing the MPP in different contexts but most of them 
fail to verify all the aforementioned properties. 

At first, in the case of specific classes of driftless nonholonomic systems (i.e. where more is known 
than the sole LARC), effective techniques have been proposed, among which a Lie bracket method 
for steering nilpotentizable systems (see [24] and [25]), sinusoidal controls for chained- form systems 
(see |29j), averaging techniques for left-invariant systems defined on a Lie group (see [26J H]), and a 
trajectory generation method for flat systems (see [II]). Depending on the applications, these methods 
turn out to be extremely efficient, especially when the system to be steered is shown to be flat with an 
explicit flat output. 

However, the class of systems considered previously is rather restrictive: for 2-input nonholonomic 
systems (i.e. m — 2), under suitable regularity assumptions, a flat system admits a feedback chained- 
form transformation (cf. (3HI I2B]) and thus is exactly nilpotentizable; on the other hand, when the 
dimension of the state space is large enough, exact nilpotentizability is clearly a non generic property 
among 2-input nonholonomic systems. Moreover, there exist standard nonholonomic systems whose 
kinematic model does not fall into any of the aforementioned categories. For instance, mobile robots 
with more than one trailer cannot be transformed in chained-form unless each trailer is hinged to the 
midpoint of the previous wheel axle, an unusual situation in real vehicles. Another similar example 
is the rolling-body problem: even the simplest model in this category, the so-called plate-ball system, 
does not allow any chained-form transformation and is not flat. 

Regarding general nonholonomic systems, various steering techniques have been proposed in the 
literature, and we only mention three of them: the iterative method, the generic loop method, and 
the continuation method. The first one, introduced in [23] and improved in [23], is an approximation 
procedure which is exact for nilpotent systems. This method is proved to be convergent with the sole 
assumption of the LARC and actually meets most of the requirements to be a complete procedure in 
the sense defined previously. However, either the resulting trajectories in [25] contain a large number 
of cusps (exponential with respect to the degree of nonholonomy) , or the computation of the steering 
control in [23] requires the inversion of a system of algebraic equations. The latter turns out to be 
numerically intractable as soon as the dimension of the state is larger than six. Let us also mention 
a less important limitation for practical use. The iterative method described in [23J [23J makes use of 
several nonlinear changes of coordinates, which must be performed by numerical integration of ODEs 
at each step of the iterative method, thus leading to spurious on-line computations. 

The generic loop method, presented in [33], is based on a local deformation procedure and requires 
an a priori estimate of some "critical distance" which is, in general, an unknown parameter in practice. 
That fact translates into a severe drawback for constructing a globally valid algorithm. The continuation 
method of [30] and [9] belongs to the class of Newton-type methods. Proving its convergence amounts to 
show the global existence for the solution of a non linear differential equation, which relies on handling 
the abnormal extremals associated to the control system. That latter issue turns out to be a hard one, 
see JTUJ [TT] for instance. This is why, in the current state of knowledge, the continuation method can 
be proved to converge only under restrictive assumptions (see [7J [HI IT2]). 

The algorithm considered in the present paper takes as starting point the globally convergent 
algorithm for steering regular nonholonomic systems discussed in [21]. As the iterative method of [231 
123] . that algorithm can be casted in the realm of nonlinear geometric control and roughly works as 
follows: one first solves the motion planning problem for a control system which is nilpotent and 
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"approximates" system (II. ip in a suitable sense; then, one applies the resulting input u to (II. ip and 
iterates the procedure from the current point. If we use x(t, a, u), t G [0, T] to denote a trajectory 
of the "approximate" control system starting from a, a local version of this algorithm is summarized 
below, where d is an appropriate distance (to be defined in the next section) and e is a fixed positive 
real number. 

Algorithm 1 Local Steering Algorithm 
Require: x$, xi, e 

k = 0; 

x k = Xo; 

while d(x k ,xi) > e do 
Compute u k such that X\ 
x k+i _ AppSteer (x k , xi) 
k = k + 1; 
end while 



We note that Algorithm[TJconverges locally provided that the function AppSteer is locally contractive 
with respect to the distance d, i.e., for x\ G Q, there exists e Xl > and c Xl < 1 such that 

cf (AppSteer (a; , x±), X\) < c Xl d(xi,x), (1.2) 

for igD and d(x±,x) < e xi . 

Assume now that we have a uniformly locally contractive function AppSteer on a connected compact 
set K C Q, i.e. there exists Ek > and ck G (0, 1) < 1 such that 

<i(AppSteer(x, Xi), Xi) < cxd(xi, x), (1.3) 

for x, X\ G K and d(xi,x) < Ek- 

Based on the local algorithm, a global approximate steering algorithm on K can be built along the 
line of the following idea (a similar procedure is proposed in [23]): consider a parameterized path 7 C K 
connecting x to x\. Then pick a finite sequence of intermediate goals {xq = Xq, xf, . . . = x\\ on 7 
such that d(xf_ l ,xf) < £k/2, i — 0, . . . , j. One can prove that the iterated application of a uniformly 
locally contractive AppSteer (x i_1 , xf) from the current state to the next subgoal (having set xf = Xi, 
for i > j) yields a sequence x l converging to X\. 

To turn the above idea into a practically efficient algorithm, three issues must be successfully 
addressed: 

(P-l) Construct a uniformly locally contractive local approximate steering method. 

(P-2) The "approximate" control u k must be exact for steering the "approximate system" from the 
current point x k to the final point X\. As this computation occurs at each iteration, it must be 
performed in a reasonable time. 

(P-3) Since the knowledge of the "critical distance" Ek is not available in practice, the algorithm should 
achieve global convergence without explicit knowledge of Ek- 

Issues (P-l) and (P-3) are solved in [21] under the assumption that the control system is regular 
(cf. Definition 12.91 below). The solution proposed therein relies on the understanding of the geometry 
defined by the nonholonomic system (cf. [3j). That geometry is a sub-Riemannian one and it endows 



= x(T, x k ,u k ); 
:= x(T, x k , u k )\ 
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the working space f2 with a sub-Riemannian metric d for which the aforementioned function AppSteer 
is contractive. Moreover, the approximation of the original system adapted to the motion planning 
turns out to be the approximation at the first order with respect to d (cf.[2T]). However, the regularity 
assumption for the control system is rather restrictive: in general, nonholonomic systems do exhibit 
singularities (cf. [12])- A solution also exists in the case of a non regular control system [H], but 
only when the state dimension n is less than or equal to 5. In the present paper, we completely 
remove the regularity assumption and solve Issues (P-l) and (P-3) for every nonholonomic control 
system. The solution is based on an explicit desingularization procedure: adding new variables (thus 
augmenting the dimension of the state space), we construct a "lifted" control system which is regular 
and whose projection is the original control system. The solution of Issue (P-l) described in [21] can 
thus be applied to the "lifted" control system, as well as the globally convergent motion planning 
algorithm solving Issue (P-3) proposed therein. Note that other desingularization procedures already 
exist [21 [13 EHl [33], but we insist on the fact that the one we propose here involves only explicit 
polynomial transformations. It numerically translates to the fact that these changes of variables can 
be performed off-line once each local procedure is identified. 

As regards Issue (P-2), several algorithms were proposed for computing u, i.e. for controlling nilpo- 
tent systems. In [21], the authors make use of piecewise constant controls and obtain smooth controls 
by imposing some special parameterization (namely by requiring the control system to stop during the 
control process). In that case, the smoothness of the inputs is recovered by using a reparameterization 
of the time, which cannot prevent in general the occurrence of cusps or corners for the corresponding 
trajectories. However, smoothness of the trajectories is generally mandatory for robotic applications. 
Therefore, the method proposed in [21] is not adapted to such applications. In [25J, the proposed 
controls are polynomial (in time), but an algebraic system must be inverted in order to access to these 
inputs. The size and the degree of this algebraic system increase exponentially with respect to the 
dimension of state space, and there does not exist a general efficient exact method to solve it. Even 
the existence of solutions is a non trivial issue. Furthermore, the methods [21] and [25] both make use 
of exponential coordinates which are not explicit and thus require in general numerical integrations of 
nonlinear differential equations. That prevents the use of these methods in an iterative scheme such as 
Algorithm [TJ Let us also mention the path approximation method by Liu and Sussmann [27], which 
uses unbounded sequences of sinusoids. Even though this method bears similar theoretical aspects 
with our method (see especially the argument strategy in order to prove Lemma I5TBI which is borrowed 
from [27]), it is not adapted from a numerical point of view to the motion planning issue since it 
relies on a limit process of highly oscillating inputs. In the present paper, we present an exact steering 
algorithm for general nilpotent systems is provided, which uses sinusoidal inputs and which can be 
applied for controlling the approximate (nilpotent) system used in [21]. Our method generalizes the 
one proposed in [2H] for controlling chained- form systems, which is briefly recalled next: after having 
brought the system under a "canonical" form, the authors of [2H] proceed by controlling component 
after component by using, for each component, two sinusoids with suitably chosen frequencies. In the 
present paper, we show for general nilpotent systems that, with more frequencies for each component, 
one can steer an arbitrary component independently on the other components. We are also able to 
construct inputs which give rise to C 1 — trajectories. 

We now describe in a condensed manner the global motion planning strategy developed in this paper. 
The latter is presented as an algorithmic procedure associated with a given nonholonomic system (£) 
defined on Q C M. n . The required inputs are initial and final points x imtial and x final belonging to Q, 
a tolerance e > 0, and a compact convex set K C VL (of appropriate size) equal to the closure of its 
interior which is a neighborhood of both x imtial and x al . For instance, K can be chosen to be a large 
enough compact tubular neighborhood constructed around a curve joining x imtial and a; final . The global 
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steering method is summarized in Algorithm |2j 



Algorithm 2 Global Approximate Steering Algorithm: Global(x imtial , x final , e, K) 
1: Build a decomposition of K into a finite number of compacts sets Vj., with i = 1, . . . ,M (Sec- 
tion 133]) . 

2: Construct the connectedness graph G := (N, E) associated with this decomposition and choose a 

simple path p := {j , ji ■ ■ - Jm} m G sucl1 that x initial G Vj. q and x final G (Section f373|) . 

3: Choose a sequence (x J )i=i,...,M_i such that x l G Vj-. fl Vj-. . 
4: Set x := x initial . 
5: for i = 1, . . . ,M- 1 do 

6: Apply the Desingularization Algorithm at a := x % with J7" := J7i (Section 13.3 j) . 

{the output is an m-tuple of vector fields £ on x R™ which is free up to step r.} 
7: Let AppSteer be the LAS method associated to the approximation of £ on Vj ; x R n defined 

in Section H~T1 and to its steering law Exact mjr constructed in Section lo"75|) . 
8: Set x Q := (x, 0), x 1 := (x\ 0), and V c := Vj. x -8^(0) with R > large enough. 
9: Apply GlobalFree(Jo, xi, e, V c , AppSteer) to £ (Section l4.2p . 

{the algorithm stops at a point x which is e-close to x±;} 
10: return x := ir(x). 

{n : Vjt x W 1 — Vj^ is the canonical projection.} 
11: end for 



The paper is devoted to the construction of the various steps of this algorithm. We will also show 
that each of these steps is conceived so that the overall construction is a complete procedure in the 
sense defined previously. In particular, the convergence issue is addressed in the following theorem. 

Theorem 1.1. Let (S) be a nonholonomic system on Q C M. n satisfying the LARC. For every e > 0, 
every connected compact set K which is equal to the closure of its interior, and every pair of points 
(x imtial ,x final ) in the interior of K , Algorithm^ steers, in a finite number of steps, the control system 
(E) from x imtial to a point x G K such that d(x,x hnal ) < e. 

Before providing the structure of the paper, we mention possible extensions of our algorithm. The 
first one concerns the working space Q. Since it is an arbitrary open connected set of R n , one can 
extend the algorithm to the case where the working space is a smooth connected manifold of finite 
dimension. From a numerical point of view, there would be the additional burden of computing the 
charts. A second extension deals with the stabilization issue. Indeed, at the heart of the algorithm 
lies an iterative procedure such as Algorithm [TJ which can be easily adapted for stabilization tasks 
(cf. [31J). Another possible generalization takes advantage of devising from our algorithm a globally 
regular input, one can then address the motion planning of dynamical extensions of the nonholonomic 
control systems considered in the present paper. Finally, let us point out the modular nature of 
Algorithm [2j one can propose other approaches to obtain uniformly contractive local methods (other 
desingularization methods or different ways of dealing with singular points), or replace Exact mjr (-) by 
more efficient control strategies for general nilpotent systems. 

The paper is organized as follows. In Section [21 we define properly the notion of first order approx- 
imation. We then propose, in Section [21 a purely polynomial desingularization procedure based on a 
lifting method. In Section HI we describe in detail the globally convergent steering algorithm given in 
[2T] for regular systems together with a proof of convergence. In Section [51 we present an exact steering 
procedure for general nilpotent systems using sinusoids, and we gather, in the appendix, the proof of 
Theorem 11.11 and some additional comments. 
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2 Notations and Definitions 



Let n and m be two positive integers. Let Q and VF(Q) be respectively an open connected subset 
of R n and the set of C°° vector fields on Q. Consider m vector fields Xi, . . . , X m of VF(Q), and the 
associated driftless control-affine nonholonomic system given by 



where u = (ui,-- - ,u m ) G M m and the input u(-) = (ui(-) , . . . , u m (-)) is an integrable vector-valued 
function defined on [0,T], with T a fixed positive real number. 

We also assume that (12. ip is complete, i.e., for every a G Q and input «(•), the Cauchy problem 
defined by (12. ip starting from a at t = and corresponding to u(-) admits a unique (absolutely 
continuous) solution x(-,a, u) defined on [0,T] and called the trajectory of (12. ip starting from a at 
t = and corresponding to the input «(■)• A point x G Q is said to be accessible from a if there exists 
an input u : [0,T] — > R m and a time t G [0,T] such that x = x(t,a,u). Then, System (12. ip is said to 
be completely controllable if any two points in Q are accessible one from each other (see [37]). 

We next provide a classical condition ensuring that System (12. ip is controllable. We first need the 
following definition. 

Definition 2.1 (Lie Algebraic Rank Condition (LARC)). Let L(X) be the Lie algebra generated by 
the vectors fields X 1; . . . ,X m and L(x) be the linear subspace of IR n equal to the evaluation of L(X) 
at every point x G Q (see [2]). If L(x) = IR n , we say that the Lie Algebraic Rank Condition (LARC for 
short) is verified at x G Q. If this is the case at every point x G Q, we say that System (12. ip satisfies 



Chow's Theorem essentially asserts that, if System (12. ip satisfies the LARC then it is completely 
controllable (cf. [IB]). 

Remark 2.1. For the sake of clarity, we assume through this paper that the control set is equal to 
M. m . However, it is well-known that Chow's theorem only requires that the convex hull of the control 
set contains a neighborhood of the origin in R m (see for instance [23J Chapter 4, Theorem 2]). We 
will explain later how we can adapt our method to the case with constraints on the control set (see 
Appendix Q3]). Moreover, it is worth recalling that complete controllability for (£) does not imply 
that LARC holds true for (S) if the vector fields Xi, . . . ,X m are only smooth, but this is the case if 
Xi, . . . , X m are analytic (cf. [U Chapter 5]). 

Throughout this paper, we will only consider driftless control-affine nonholonomic systems of the 
type (12. ip verifying the LARC, and thus completely controllable. In that context, the motion planning 
problem will be defined as follows: find a procedure which furnishes, for every two points xq, x\ G Q, 
an input u steering ( 12. ip from xo to X\, i.e., x(T, Xq,u) = x\. 

Our solution to this problem relies heavily on the underlying geometry, which is a sub-Riemannian 
geometry. We provide in Section 12.11 the useful definitions and refer the reader to [2] for more details. 
We then introduce in Section 12.21 a notion of approximate steering method related to this geometry. 



rn 




(2.1) 



»=i 



the LARC. 
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2.1 Basic facts on sub-Riemannian geometry 
2.1.1 Sub-Riemannian distance and Nonholonomic order 
Definition 2.2 (Length of an input). The length of an input u is defined by 

£(u) = £ y/ul(t) + ---+U* m {t)dt, 

and the length of a trajectory x(-, a, u) is defined by £(x(-, a, u)) := £(u). 

The appropriate notion of distance associated with the control system (12. ip and closely related to 
the notion of accessibility is that of sub-Riemannian distance, also called control distance. 

Definition 2.3 (Sub-Riemannian distance). The vector fields Xi, . . . ,X m induce a function d on Q, 
defined by d(xi,x 2 ) '■— mi u £(x(-,Xi,u)), for every points Xi,x 2 in Q, where the infimum is taken over 
all the inputs u such that x(-,xi,u) is defined on [0,T] and x(T, Xi,u) = x 2 . We say that the function 
d is the sub-Riemannian distance associated with X±, . . . , X m . 

Remark 2.2. The function d defined above is a distance in the usual sense, i.e., it verifies (i) d(x\, x 2 ) > 
and d(xi, x 2 ) = if and only if x\ = x 2 , (ii) symmetry: d(x\, x 2 ) = d(x 2 , x\); (iii) triangular inequality: 
d(xi,x 3 ) < d(xi,x 2 ) + d(x 2 ,x 3 ). Notice that one always has d(xi,x 2 ) < oo since the control system is 
assumed to be completely controllable. 

Definition 2.4 (Nonholonomic derivatives of a function). If / : Q — > W 1 is a smooth function, the 
first-order nonholonomic derivatives of / are the Lie derivatives X{f of / along Xi, i = l,...,m. 
Similarly, Xi(Xjf), i,j = l,...,m, are called the second-order nonholonomic derivatives of /, and 
more generally, X ix ■ ■ ■ X ik f , i 1: . . . , i k G {1, . . . , m} are the k th -order nonholonomic derivatives of /, 
where k is any positive integer. 

Proposition 2.1 ([21 Proposition 4.10, page 34]). Let s be a non-negative integer. For a smooth 
function f defined near a G Q, the following conditions are equivalent: 

(i) f(x) = 0(d s (x,a)) for x in a neighborhood of a; 

(ii) all the nonholonomic derivatives of order < s — 1 of f vanish at a. 

Definition 2.5 (Nonholonomic order of a function). Let s and / be respectively a non-negative integer 
and a smooth real- valued function defined on Q. If Condition (i) or (ii) of Proposition 12.11 holds, we 
say that / is of order > s at a G Q. If / is of order > s but not of order > s + 1 at a, we say that / is 
of order s at a. The order of / at a will be denoted by ord a (/). 

Definition 2.6 (Nonholonomic order of a vector field). Let q be an integer. A vector field Y G VF(Q) 
is of order > q at a G Q if, for every non-negative integer s and every smooth function / of order s at 
a, the Lie derivative Y f is of order > q + s at a. If Y is of order > q but not > q + 1, it is of order q 
at a. The order of Y at a will be denoted by ord a (Y). 

Definition 2.7 (Nonholonomic first order approximation at a). An m-tuple X a := {X^, . . . ,X^}, 
defined on B(a,p a ) := {x G Q, d(x,a) < p a } with p a > 0, is said to be a nonholonomic first order 
approximation of X := {Xi, . . . , X m } at a G Q, if the vector fields Xi — X?, for i — 1, . . . , m, are of 
order > at a. The positive number p a is called the approximate radius at a. 

Remark 2.3. As a consequence of Definition 12. 7\ one gets that the nonholonomic order at a defined 
by the vector fields Xf , . . . , X^ coincides with the one defined by Xi, . . . , X m . 
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2.1.2 Privileged coordinates 

The changes of coordinates take an important place in this paper, whether it is to estimate the sub- 
Riemannian distance, or to compute the order of functions and vector fields, or to transform a control 
system into a normal form. To avoid heavy notations, we will need some conventions and simplifications 
that we fix now for the rest of the paper. 

A point in Q C M. n is denoted by x = (xi, . . . , x n ) and the canonical basis of W 1 by (d Xl , . . . , d Xn ). 
Even though x is a point, we will sometimes refer to {x%, . . . , x n ) as the original coordinates. A system 
of local coordinates y = {yi, . . . , y n ) at a point a G Q is defined as a diffeomorphism ip between an open 
neighborhood N a C Q of a and an open neighborhood N v ^ C M. n of <p(a), ip : x y = (yi, . . . , y n ). If 
the diffeomorphism ip is defined on Q, then y = . . . , y n ) is said to be a system of global coordinates on 
Q. A system of global coordinates is said to be affine (resp. linear) if the corresponding diffeomorphism 
ip is affine (resp. linear). If / is a function defined on N a , the function / o ip" 1 defined on iV v ( ) will 
be called / (expressed) in coordinates (yi, . . . ,y n )- If X G VF(Q) is a vector field, the push-forward 
tp*X = dip o X o ip" 1 g VF(N ip (g\) will be called X (expressed) in coordinates (yi, . . . ,y n ). 

For the sake of simplicity, we will in general not introduce the notation ip and, with a slight abuse 
of the notation, replace it by y. Thus we write y(x) or (yi(x), . . . , y n (x)) instead of <p(x). The function 
/ o ip -1 will be denoted by f(y), and the vector field <p*X by X(y) or y*X. The values at a point 
y G ^(o) will be denoted respectively by f(y)\ y =y and X(y)\ y= y. 

A special class of coordinates, called privileged coordinates and defined below, turns out to be a 
useful tool to compute the order of functions and vector fields, and to estimate the sub-Riemannian 
distance d. 

We will use L S (X) to denote the Lie sub-algebra of elements of length (cf. Definitions 13.11 and 13 ,4p 
not greater than s6N. Take and let L s (x) be the vector space generated by the values at x of 

elements belonging to L S (X). Since System (12.11) verifies the LARC at every point x G Q, there exists 
a smallest integer r := r(x) such that dimL r (x) = n. This integer is called the degree of nonholonomy 
at x. 

Definition 2.8 {Growth vector). For a G f2, let n s (a) := dimL s (a), s = l,...,r. The sequence 
(ni(a), . . . , n r (a)) is the growth vector of X at a. 

Definition 2.9 (Regular and singular points). A point a G Q is said to be regular if the growth vector 
remains constant in a neighborhood of a and, otherwise, a is said to be singular. The nonholonomic 
System fl2.ll) (or the m-tuple X) is said to be regular if every point in Q is regular. 

Note that regular points form an open and dense set in Q. 

Definition 2.10 (Weight). For a G f2 and j = 1, ...,n, let u^- := Wj(a) be the integer defined by 
setting Wj := s if n s _i < j < n s , with n s := n s (a) and n := 0. The integers Wj, for j = 1, . . . , n are 
called the weight at a. 

Remark 2.4. The meaning of Definition 12.101 can be understood in another way. Choose first some 
vector fields W±, . . . , W ni in L l (X) such that Wi(a), . . . , W m (a) form a basis of L l (a). Choose then 
other vectors fields W ni+ i, . . . , W n2 in L 2 (X) such that Wi(a), . . . , W n2 (a) form a basis of L 2 (a) and, for 
every positive integer s, choose W ns _ 1+ i, . . . , W ns in i^ s (A) such that Wi(a), . . . , W ns (a) form a basis 
of L s (a). We obtain in this way a sequence of vector fields Wi, . . . , W n such that 

J Wi(a), . . . , W n (a) is a basis of R n , , , 

\ Wi G L w *(X),i = l,...,n. ^ ' 
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A sequence of vector fields verifying Eq. (12. 2p is called an adapted frame at a. The word "adapted" 
means "adapted to the flag L l (a) C L 2 (a) C ■ ■ • C L r (a) = R n ", since the values at a of an adapted 
frame contain a basis Wi(a), . . . , W ns (a) of every subspace L s (a) of the flag. The values of Wi, . . . , W n 
at a point b close to a also form a basis of R n . However, if a is singular, this basis may be not adapted 
to the flag L l {b) C L 2 (b) C ■ ■ ■ C L r ^(&) = R n . 

Definition 2.11 [Privileged coordinates at a). A system of privileged coordinates at a G is a system 
of local coordinates (zi, . . . , z n ) centered at a (the image of a is 0) such that ord a (zj(x)) = Wj, for 

j = 1, ... ,71. 

Remark 2.5. For every system of local coordinates (yi, . . . ,?/„) centered at a, we have, up to a re- 
ordering, ord a (?/j) < Wj or, without re-ordering, YTj=i OT( ^-a{yj) < S^i^i- 

The order at a G of functions and vector fields expressed in a system of privileged coordinates 
(zi, . . . , z n ) centered at a can be evaluated algebraically as follows: 

• the order of the monomial z" 1 . . . z% n is equal to its weighted degree w(a) := W\a\ + ■ • • + w n a n ; 

• the order of a function f(z) at z = is the least weighted degree of the monomials occurring in 
the Taylor expansion of f(z) at 0; 

• the order of the monomial vector field z" 1 . . . z^ n d z . is equal to its weighted degree w(a) — Wj, 
where one assigns the weight — Wj to d Zj at 0; 

• the order of a vector field W(z) = Y^j=i Wj(z)d Zj at z = is the least weighted degree of the 
monomials occurring in the Taylor expansion of W at 0. 

Definition 2.12 (Continuously varying system of privileged coordinates). A continuously varying sys- 
tem of privileged coordinates on Q is a mapping $ taking values in IR n , defined and continuous on a 
neighborhood of the set {(x,x),x G Q} C Q x Q, and so that the partial mapping z := $(a, •) is a 
system of privileged coordinates at a. In this case, there exists a continuous function p : O — > (0, +oo) 
such that the coordinates $(o, ■) are defined on B(a,p(a)). We call p an injectivity radius function of 

Definition 2.13 (Pseudo-norm) . Let a G Q and wi, . . . , w n the weights at a. The application from IR n 
to R defined by \\z\\ a := Izi] 1 ^ 1 + • • ■ + l^l 1 /™", z = (zi, . . . , z n ) G R n , is called the pseudo-norm at a. 



2.1.3 Distance and error estimates 

Privileged coordinates provide estimates of the sub-Riemannian distance d, according to the following 
result. 

Theorem 2.2 (Ball-Box Theorem [2J). Consider (X 1 ,...,X m ) G VF(tt) m , a point a G SI, and a 
system of privileged coordinates z at a. There exist positive constants Cd(a) and Sd(a) such that, for 
every i6ll with d(a,x) < Sd(a), one has 

777 \ IkC^IU < d(a,x) < C d (a) \\z(x)\\ a . (2.3) 

If Q contains only regular points and if $ is a continuously varying system of privileged coordinates 
on Q, then there exist continuous positive functions Cd(-) and Ed(-) on Q such that Eq. i2.3\) holds true 
with z = $(a, •) at all (x, a) satisfying d(x, a) < Sd(a). 
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Corollary 2.3. Let K be a compact subset of Q. Assume that K only contains regular points and 
there exists a continuously varying system of privileged coordinates $ on K . Then, there exist positive 
constants Ck and Ek such that, for every pair (a,x) G K x K verifying d(a,x) < ek, one has 

— \\®(a,x)\\ a < d{a,x) < C K ||$(a, x)|| a . (2.4) 

Privileged coordinates also allow one to measure the error obtained when X is replaced by an 
approximation X. 

Proposition 2.4 ([21 Prop. 7.29]). Consider a point a &Vt, a system of privileged coordinates z at a, 
and an approximation X of X at a. Then, there exist positive constants C e (a) and e e (a) such that, for 
every i6ll with d(a,x) < e e (a) and every integrable input function «(•) with i(u) < E e (a), one has 

\\z(x(T,x,u))-z(x(T,x,u))\\ a < C7 e (a)max(||z(x)|| ^(u)) £(u) 1/r , (2.5) 

where r is the degree of nonholonomy at ) and x(-,x,u) are respectively the trajectories of 

x = J2lLi u i X i( x ), andx = Yn=i u i x i{ x ) ■ 



2.2 Approximate steering method 

Definition 2.14 (Nonholonomic first order approximation on Q). A nonholonomic first order approx- 
imation of X on Q is a mapping A which associates, with every a G f2, a nonholonomic first order 
approximation of X at a defined on B(a,p a ), i.e., A(a) := X a on B(a,p a ). The approximation radius 
function of A is the function p : Q — > (0, oo) which associates, with every a, its approximate radius p a , 
i.e., p(a) := p a . 

In the sequel, nonholonomic first-order approximations will simply be called approximations. Useful 
properties of approximations are continuity and nilpotency. 

Definition 2.15 (Continuity and nilpotency of an approximation). Let A : a X a be an approxima- 
tion on Q. 

• We say that A is continuous if 

(i) the mapping (a,x) h-> A(a)(x) is well-defined and, for every a G Q, is continuous on a 
neighborhood of (a, a) G f2 x Q; 

(ii) the approximation radius function p of A is continuous. 

• We say that A is nilpotent of step s G N if, for every a G f2, the Lie algebra generated by X a is 
nilpotent of step s, i.e. every Lie bracket of length larger than s is equal to zero. (For a definition 
of the length of a Lie bracket, see Definitions 13.11 and 13 . 41 ) 

Consider a m-tuple of vector fields X = {X±, . . . , X m } in VF m (Q). 

Definition 2.16 (Steering law of an approximation). Let A : a h-> X" be an approximation of X 
on and p its approximation radius function. A steering law of A is a mapping which, to every 
pair (x,a) G Q x £7 verifying a) < p(a), associates an integrable input function u : [0, t] h-> M m , 
henceforth called a steering control, such that the trajectory x(-, x, -u) of the approximate control system 

m 

A = ^A7(x), (2.6) 
i=i 

is defined on [0,T] and satisfies x(T,x,u) = a. In other words, «(■) steers (12.61) from x to a. 
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A steering law of an approximation is intended to be used as an approximate steering law for the 
original system. For that purpose, it is important to have a continuity property of the steering control: 
the closest are x and a, the smaller is the length of u. We introduce the stronger notion of sub-optimality 
(which is a sort of Lipschitz continuity of the steering law). 

Definition 2.17 (Sub- optimal steering law). Let A be an approximation of X on Q and, for every 
a £ Q, let d a be the sub-Riemannian distance associated to A(a). We say that a steering law of A is 
sub-optimal if there exists a constant Ce > and a continuous positive function se(-) such that, for any 
a,x Efl with d(a,x) < £g(a), the control u(-) steering (12. 6p from x to a satisfies: £(u) < Cid a (x,a) = 
Ci d a (x(0, x, u),x(T, x, u)). 

Due to the definition of the sub-Riemannian distance d a , sub-optimal steering laws always exist. 
Given an approximation A of X and a steering law for A, we define a local approximate steering method 
for X as follows. 

Definition 2.18 (Local approximate steering). The local approximate steering (LAS for short) method 
associated to A and its steering law is the mapping AppSteer(-, •) which associates, with every pair 
(x, a) E Q x Q verifying d(x,a) < p(a), the point x(T, x, u), i.e., 



where u(-) is the steering control of A(a) associated to (x, a) and p is the approximation radius function 



Definition 2.19 (Local contractions and uniform local contractions). A LAS method is locally con- 
tractive if, for every a e fl, there exist e a > and c a < 1 such that one has: 



A LAS method is uniformly locally contractive on a compact set K C Q if it is locally contractive, 
and if e a and c a are independent of a, i.e., there exists Ek > and ck < 1 such that, for every pair 
(a, x) G K x K, the following implication holds true: 



Remark 2.6. We will show that if X is an approximation of X at a, the corresponding AppSteer 
function is locally contractive in a neighborhood of a. By the Fixed Point Theorem, one gets local 
convergence of Algorithm 1 (LAS). However, in order to obtain a globally convergent algorithm from 
LAS, one needs AppSteer to be uniformly locally contractive. In other words, the mapping A needs to 
be continuous in the sense of Definition 12.151 

As a direct consequence of Proposition 12.41 we obtain sufficient conditions for a LAS method to be 
uniformly locally contractive. 

Corollary 2.5. Let K be a compact subset ofQ. Assume that: 
(i) all points in K are regular; 

(ii) there exists a continuously varying system of privileged coordinates $ on K; 



AppSteer(x, a) := x(T,x,u), 



of A. 
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(Hi) there exists a continuous approximation A of X on K; 



(iv) A is provided with a sub-optimal steering law. 

Then, the LAS method AppSteer associated to A and its steering law is uniformly locally contractive. 
Moreover, up to reducing the positive constant Ek occurring in Corollary \2.3[ one has, for every pair 
(a,x) G K x K verifying d(a,x) < Ek, 



Proof of Corollaru \2.5\ Under the hypotheses (i) — (iv), one immediately extends Proposition 12.41 and 
obtains that there exist continuous positive functions C e (-) and E e (-) such that inequality (I2.5P holds 
true, with z = $(a, •) and X = A(a), for every pair (x, a) G Q x Q with d(x,a) < E e (a) and every 
integrable input function u(-) with £(u) < E e (a). The remaining argument is standard and one conclude 
easily. 



Remark 2.7. Since the growth vector and the weights do not remain constant in any open neighbor- 
hood of a singular point, privileged coordinates z cannot vary continuously in any open neighborhood 
of that singular point. Therefore, around a singular point, the distance estimations provided in Eqs. 
(12. 4p and (12.81) and based on privileged coordinates do not hold true uniformly. In particular, if (a n ) 
is a sequence of regular points converging to a singular point a (this is possible since regular points 
are dense in Q), the sequences Ed(a n ) and E e (a n ) tend to zero whereas Ed(a) and £ e (a) are not equal to 
zero. 

Remark 2.8. A similar discontinuity issue occurs of course for the approximate system. Indeed, if 
a is a singular point, the growth vector and the weights of the associated privileged coordinates at a 
change around a, implying a change of the truncation order in the Taylor expansion of the vector fields. 
Therefore, the approximate vector fields cannot vary continuously in any neighborhood of a singular 
point. 

3 Desingularization by Lifting 

As it appears in Corollary 12. 5[ the absence of singular points is one of the key features in order to 
construct uniformly locally contractive LAS method. As a matter of fact, we will show in Section l4~2l 
how to construct a globally convergent motion planning algorithm for a regular nonholonomic system 
(i.e., when all points in Q are regular). 

However, in general, nonholonomic systems do have singular points. For such systems, attempts 
have been made to construct specific LAS methods (see [JH [22]), but additional conditions on the 
structure of the singularities are required. Our approach here is different: we present in this section a 
desingularization procedure of the system, in such a way to replace a MPP for a non regular system 
by a MPP for a regular one. 

The strategy consists in "lifting" the vector fields {Xi,...,X m } G VF m (VL) defining the control 
system to some extended domain Q := Q x MJ 1 , with n G N to be defined later. The lifted vector fields 
{£i, . . . , £ m } G VF m (Q) are constructed so that: 



<i(AppSteer(x, a), a) < -d(x,a), 
2;(AppSteer(x,a))|| a < ^ |Kz)|| . 



(2.7) 



(2.8) 



□ 
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(i) for i — 1, . . . , m, & has the following form, 



£i(x, y) = Xi(x) + M s > v) d vi> ( x > f) el]xRi . 

3=1 

where the functions by, j = 1, . . . , h, are smooth; 

(ii) the Lie algebra generated by . . . ,£ m } is free up to step r (see Def. 13.61 below). 

Point (ii) guarantees that the nonholonomic system defined by {£i, . . . , £ m } is regular, since its 
growth vector is constant on Q. Point (i) guarantees that one obtains X±, . . . ,X m by projecting 
£i, . . . ,£ m on H^ n - Indeed, let ir be the canonical projector from Q to Q defined by tt(x) = x, where 
x = (x, y) G £1. Then, denoting dn^ the differential of 7r at x, one has 

As a consequence, the projection by n of a trajectory x(-,xo,u) of the control system 

m 

x = y]ui£i(x),x e (3.1) 
i=i 

is a trajectory of ( 12 .ip associated to the same input, i.e., tt(x(-, Xq, u)) = x(-,7r(x ),u). 

Therefore, any control u steering System (13. ip from a point xo := (xq,0) to a point X\ := (xi,0) 
also steers System (12. ip from xo to X\. It then suffices to solve the MPP for the regular System ( 13. ip . 

Note that distinguished desingularization procedures already exist, cf. [331 El ED]- However, an 
important property of the desingularization procedure presented here is that all the changes of coor- 
dinates and intermediate constructions involved in it are explicit and purely algebraic. Note also that, 
during the lifting process, we obtain, as a byproduct, a nonholonomic first order approximation of 
{£i, . . . ,£ m } in a "canonical" form, which can be exactly controlled by sinusoids (see Section E}. 

We start this section by presenting some general facts on free Lie algebras, namely the P. Hall basis 
in Section 13.11 and the canonical form of a nilpotent Lie algebra of step r in Section 13.21 We then give 
one desingularization procedure in Section 13.31 The proofs of the results stated in Section 13.31 will be 
gathered in Section [331 

3.1 P. Hall basis on a free Lie algebra and evaluation map 

In this section, we present some general facts on free Lie algebras. The reader is referred to [B] for 
more details. Consider X := {1, . . . ,m}, and the free Lie algebra C(X) generated by the elements of 
X. Recall that £(X) is the M- vector space generated by the elements of X and their formal brackets 
[ , ], together with the relations of skew-symmetry and the Jacobi identity enforced (see [6] for more 
details). We note that, by construction, for every / £ £(X), there exists £ £(X) X £(X) such 

that I = [h,I 2 ]- 

Definition 3.1 (Length of the elements of £(Z)). The length of an element / of a free Lie algebra 
£(X), denoted by |J|, is defined inductively by 

\I\ := 1, for I = l,...,m; (3.2) 
|/| := |/i| + |/ 2 |, for I=[h, hi with/i, I 2 eL(X). (3.3) 
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We use £ S (X) to denote the subspace generated by elements of £(X) of length not greater than s. 
Let n s be the dimension of C S (X). 

The P. Hall basis of £(X) is a totally ordered subset of £(X) defined as follows. 

Definition 3.2 (P. Hall basis). A subset H = {Ij} je N of £(X) is the P. Hall basts of £(X) if (HI), 
(H2), (H3), and (H4) are verified. 

(HI) If < \Ij\, then k -< If, 

(H2) {1, . . . , m} C H, and we impose that 1 -< 2 -<••■-< m; 

(H3) every element of length 2 in "H is in the form [Ij, ij] with (/«, Jy) G X x X and -< Jj-; 

(H4) an element Ik G £(X) of length greater than 3 belongs to % if Ik = [1^ > l/fa ? ^fe 3 ]] with J/ Cl , Ik 2 , Ifc 3 , and 
belonging to H, I k2 -< I ks , I k2 -< I kl or I k2 = I kl , and I kl -< [I k2 , I ks ). 

The elements of H form a basis of £(X), and " -< " defines a strict and total order over the set H. In 
the sequel, we use I k to denote the k th element of H with respect to the order " -< ". Let H s be the 
subset of H of all the elements of length not greater than s. The elements of H s form a basis of £ S (X) 
and Card("H s ) = n s . The set Q s := % s \ % s ~ l contains the elements in % of length equal to s. Its 
cardinal will be denoted by k s = Card((? s ). 

By (HI) — (H4), every element Ij G "H can be expanded in a unique way as 

/,- = [/ fcl ,[4 2 ,-.-,[/ fci , 4] •-•]], (3.4) 

with ki > ■ ■ ■ > ki, k{ < k, and k G {1, . . . , n{\. In that case, the element Ij is said to be a direct 
descendant of Ik, and we write <p(j) := /c. For G "H r , the expansion (13.41) also associates with Ij G "H 
a sequence = (a], . . . , a" r ) in Z rar defined by 

a l j := Card {s G {1, . . . , i}, k s = £}. 

By construction, one has = for £> j, and <x,- = (0, . . . , 0) for 1 < j <n\. 

Consider now a family of m vector fields X = {Xi, . . . , X m } and the Lie algebra L(X) they generate. 
The P. Hall basis H induces, via the evaluation map, a family of vector fields spanning L(X) as a linear 
space. 

Definition 3.3 (Evaluation map). The evaluation map Ex defined on C(I), with values in L(X), 
assigns to every / G £(X) the vector field Xj = E X (I) obtained by plugging in X iy i = 1, . . . , m, for 
the corresponding letter i. 

Definition 3.4 (Length of the elements of L(X)). With the notations of Definition [331 if — Ex(I), 
the length of Xj, denoted by A(Xi), is set to be equal to \I\, . 

Definition 3.5 (P. Hall family). The P. Hall family Hx associated with the vector fields X = 
{Xi, . . . ,X m } is defined by H x := {E X (I),I G H}, where E x is the evaluation map and % is the 
P. Hall basis of the free Lie algebra C(X). The family Hx inherits the ordering and the numbering of 
the elements in H induced by (H1)-(H4). 

Note that Hx is a spanning set of L(X), but not always a basis. 

Definition 3.6 (Free up to step s). Let s be a positive integer such that 1 < s < r. A family of vector 
fields £ = {£i, . . . , £ m } defined on a subset Q of R" r is said to be free up to step s if, for every x G Q, 
the growth vector (ni(x), . . . ,n s (x)) is equal to (n 1 , . . . , n s ). 
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Remark 3.1. If £ defined on Q C M" r is free up to step r, then every point of Q is regular. 

Definition 3.7 (Free weights). Let £ = {£1, . . . ,£ m } be free up to step r on Q C M nr . The integers 
Wi, . . . , t?n r , where SJj = s if n a _i(£) < j < n s (5f) for every !r G f2, are called the free weights of step r. 

3.2 Canonical form 

We present in this section the construction of a canonical form for nilpotent systems proposed by 
Grayson and Grossman in [16] and [17] . Similar results were also obtained by Sussmann in |37j . 

The construction takes place in K nr , where r is a positive integer and n r the dimension of £ r (I). 
We denote by v = (f i, . . . ,fn r ) the points in R nr , and by (d Vl , . . . , <9„~ ) the canonical basis of R nr . 
For j = 1, . . . ,n r , we assign to the coordinate function Vj the weight Wj and to the vector d v . the 
weight —Wj. The weighted degree of a monomial f" 1 • • •f~" r is then defined as w(a) := wiai + • • • + 
ffiH r aH r , where a = (a%, . . . , «s r ), and the weighted degree of a monomial vector field v" 1 . . . v~ nr d Vj 
is defined as w(a) — Wj. 

For every Ij G W , let a, be the sequence associated with Ij (see Section I3TT]) . Define the monomial 
Pj(v) associated with Ij by 

Pj{v) := — (3.5) 
atj. 

where v aj := TT^j an d cnj\ := TTcfcj!. Note that P 3 - satisfies the following inductive formulas. 

£ £ 

Pj(v) = 1 HIjeH 1 ; 

Pj( V ) = -^-PjM if/, ;/,,•/,/• ^ 

Theorem 3.1 ( [TBI ITT]). We define the vector fields Dx, . . . , D m on M nr as follows: 

D 1 = d Vl , 



2<|/j|<r 

*(i)=2 

An = 0„ m + ^ Pj(v)d Vj . 

2<\Ij\<r 
4>{j)=m 

Then, the Lie algebra L(D) generated by (Di, . . . , D m ) is free up to step r, and one has 

Dj.(0)=d Vj , forIjEH r , 

where Dj. := Eu(Ij) is defined through the evaluation map Ed with values in L(D). 

The proof of Theorem 13 . 1 1 goes by induction on the length of elements in L(D). The reader is referred 
to [T7] for a complete development. 

Corollary 3.2. For all Ik G 1-L r , Di k has the following form 

Di k = d Vk + Yl P » d ^ (3-7) 

Ij&W, \Ij\>\h\ 

where every non zero polynomial is homogeneous of weighted degree equal to \Ij\ — \Ik\. 
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Remark 3.2. The explicit expression of the polynomials Pj(v) as functions of the monomials Pj(v) is 
obtained through an induction formula. 

Corollary 3.3. For i — 1, . . . , m, we define m vector fields Di as follows: 

A := d Vi + P ^ 9 ^ + E Pji?)^, 

2<|/ & |<'H r - 1 ijeS 
4>{k)=i 4>U)=i 

where S is an arbitrary non-empty subset of Q r . Then, 

• if he W- 1 U S, we have D h = d Vk + Ei^-ius, |/ 3 |>|/ fe | p ^( v ) d v/, 

• if h e Q T \ S, we have L>i k = 0. 

Definition 3.8 (Canonical form). Let X±, . . . , X m be m vector fields on an open subset Q of M nr and 
v a local system of coordinates on Q. The control system associated to {Xi, . . . ,X m } is said to be in 
canonical form in the coordinates v if one has t>*Xj = Di, for % = 1, . . . ,m, where we use t>*Xj to 
denote the push- forward of Xi by v. 

Consider now the control system given by 

rn 

v = ^2 Ul Di{v), veW lr . (3.8) 

i=l 

Writing (13 .8p component by component, one has, for j = 1, . . . , n r , 

Vj = Pj(vi_, Vj-i)ui, where i = 0(j), (3.9) 



or inductively, 
More explicitly, one has 



v ■ 

Vj = h n - v j2 , where Ij = [I h J j2 ]. (3.10) 
a h + 1 

where ad^ X r := [X/ , [Xj , • • • , [X 7 , X/ ], with X/ = [X/ X/ ] and J ia -< J^. The inductive 
n v ^ / 

k times 

formula ( 13. lip will be used in Chapter [5j 

A particular system of coordinates, called canonical coordinates (a terminology arising from Lie 
group theory), allows one to obtain canonical forms. Consider m vector fields Xi, . . . , X m on Q C M nr , 
let v G ft, and W = {Wi, . . . , W n } be a set of vector fields in L(X) such that Wi(v), . . . , W n {y) is a 
basis of M. nr . The canonical coordinates of the second kind at v associated with W are the system of 
local coordinates at v defined as the inverse of the local diffeomorphism 

( Zl , z nr ) i— »• e z ^ w *r o ■ • • o e ZlWl {v), (3.12) 

where we use e zWi to denote the flow of When the system (X 1? . . . ,X m ) is nilpotent, the above 
diffeomorphism defines global coordinates on Q for every v 6 £1. 

Theorem 3.4 (|37j). Assume that the vector fields {X\, . . . ,X m ) generate a Lie algebra which is both 
nilpotent of step r and free up to step r. Then, in the canonical coordinates of the second kind associated 
with the P. Hall basis H r x , the control system defined by (X\, . . . , X m ) is in canonical form. 

Remark 3.3. The canonical coordinates of the second kind require to determine the flow of the control 
vector fields i.e., to integrate some differential equations. In general, there does not exist algebraic 
change of coordinates between an arbitrary system of coordinates and the canonical coordinates of the 
second kind. 
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3.3 Desingularization algorithm 



Let X = {Xi, . . . , X m } C VF(Q) be a family of m vector fields on Q C M n , and K be a compact subset 
of f2. We assume that the LARC is satisfied at every point of K. Therefore, the degree of nonholonomy 
of X is bounded on K and we denote by r its maximal value. 

Recall that W denote the elements of the P. Hall basis of length smaller or equal to r. For every 
n-tuple J = (Ji, . . . , I n ) of elements of H r , we define the domain Vj C Q by 

Vj ■= { V e such that det(X 7l (p), . . . ,X /n (p)) ^ }, (3.13) 

where Xj. = E x (Ij)- Such a set Vj is open in Q (possibly empty) and for every p G Vj, the vectors 
X ix (p), . . . , X Jn (p) form a basis of M. n . 

Since K is compact, there exist a finite family of n-tuples J7i , . . . , J7m of elements of W such that 

^c(JVjj. (3.14) 

i=i 

One deduces from (13.141) a compact covering of K in the form 

Kc\JV$ if (3.15) 

i=i 

where, for i = 1, . . . , M, the set Vj. C Vj t is compact. 

Definition 3.9. Let (S^^i be a finite set of subsets of Q. The connectedness graph G := (N, E) 
associated with is defined as as follows: 

- the set of nodes N := J; 

- a pair (i, j) with i and j in N belongs to the set of edges E if Si fl Sj ^ 0. 

A simple path on G is a subset p := . . . of two by two distinct elements of N such that, for 
j = 1, . . . , L — 1, the pair belongs to E. 

Remark 3.4. With the notations of Definition 13.9} if we assume that all the sets Si are open or, all of 
them are closed, and the set S := U ie iSi is connected, then, for every (x , xi) G S x S, there exists a 
simple path on G denoted by p := {ii, . . . such that Xq G and X\ G Si L . 

Take J = (Ji, . . . , I n ) among J7i, . . . , Jm, and pick a point a in Vj. In the sequel, we construct, by 
induction on the length of elements in a free Lie algebra, a family of m vector fields £ = {£i, . . . , £ m } 
defined on V7 x R nr ~ n , which is free up to step r and has its projection on Vj equal to X. At the 
same time, we give an approximation of £ at a := (a, 0) G Vj x R nr ~ n in canonical form. 

We define J s := {Ij G J, with 1^1 = s}, for s > 1, and £ s := H s \ "H 5-1 , for s > 2. We denote 
by fc s the cardinal of J~ s , and by k s the cardinal of We are now ready to describe in details our 
desingularization algorithm. 

Desingularization Algorithm (DA) 

• Step 1: 

(1-1) Set V 1 := Vj x R^~ k \ a 1 := (a, 0) G V 1 , /C 1 := H 1 U (J \ J 1 ). 
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(1-2) Define {£,..., on V 1 as follows: 



(1-3) Compute the coordinates y 1 on V 1 defined as the unique affine system of coordinates on V 1 
such that d y i = ^(a 1 ), for Ij e /C 1 , and y 1 (a 1 ) = 0. 

(1-4) Construct the system of global coordinates z 1 on V 1 by 



z) := y), forj eft 1 , 

ni 

4 : = i/j-Etfi-viXi/V^ v*> for/.e/C^ft 1 , 



fe=l 

where Ij denotes the j th element in /C 1 . 
• Step s, 2 < s < r: 

(s-1) Set V s := V s " 1 x R h '~ k % a s := (a, 0) G V s , and /C s := /C^ 1 U (Q s \ J s ). Denote by V s the 
points in IR fcs ~ fcs . 

(s-2) Define . . . , ££j as the vector fields on V s which write in coordinates (z s_1 ,i> s ) as: 

</-(fc)=i 

(s-3) Compute the system of global coordinates y s on V s as the unique isomorphism (-2 s-1 , v s ) h- >■ y s 
such that <9j/s {/) = C|(a s ) for every I E fC s . 

(s-4) Construct the system of global coordinates z s on V s by the following recursive formulas: 
(s-4)-(a) for Ij e ft*, 



l^'l-i 



fc=2 

where, for = 2, ... , \Ij\ — 1, 
r k {y\, . . . ^y*) 



k-l 



- E [(eiJ ft ---(^,J ft - 1 -(yJ + E^]^)i^o (/yr) 



A! 

fc 9=2 K1 



«09)<|I,-| 

(s-4)-(b) for eJC s \H s , 



zs 



fc=2 
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where, for k — 2, . . . , s, 

s 

r k (yt,...,yl) = ■ ■ • (£J*. • (y* + £ r,)] (</V=o 

|/3|=fe 9=2 
ui(fi)<a 

ft! "' /%.! ' 

(s-5) Construct the system of global coordinates z s as follows: 
(s-5)-(a) for j > n s , set z| := 

(s-5)-(b) for j = 1, . . . , n s , set z| := ^ 8 Az{, . . . , where all ty s are polynomials such that the 

two following conditions are satisfied: 

- if we impose the weight of z? to be Wj for j = 1, . . . , n s , then every ty? is homogeneous 
of weighted degree equal to Wj] 

- denote by ord^(-) the nonholonomic order defined by . at a s , and by 
£ij(z s ) the j th component of £/(z s ); then one has 

C,U') = ^P^, • • • , S-i) + •/ 1 (3-18) 

where ord*« > Wj (S^k denotes the Kronecker symbol). Note that ord* s (Pj) = 
Wj - 1. 

Theorem 3.5. Let £j := for i = 1, . . . ,m, and z := z r , where £]" and z r are given by the desingu- 
larization algorithm. Then, the family of vector fields £ = {£i, . . . , £ m } defined on Q x ]R nr_n zs free up 
to step r. Moreover, the system of coordinates z = (z\, . . . , Zn r ) is a system of privileged coordinates at 
a for £ ; and the family of vector fields £ = {£i, . . . ,£ m } defined in the coordinates z by the canonical 
form: 

ii = d Zi + ^2 Pj(zu- ■ ■ ,Zj-i)d Zjl fori = l,...,m, (3.19) 

2<\Ij\<n r 
4>{j)=i 

is a nonholonomic first order approximation of £ at a. 

Remark 3.5. We note that the desingularization procedure does not a priori require that 

(a) the coordinates z are privileged coordinates; 

(b) the system £ is a first order approximation of £ at a. 

However, (a) and (b) can be used directly at the first step of the motion planning algorithm presented 
in Section |H 

Remark 3.6. If we assume that the original system X = {X\, . . . , X m } is nilpotent, then, by adapting 
the proof of Theorem 13 . 5 1 presented in Section [331 one can show that the corresponding "lifted" system 
£ = . . . ,£ m } given by the Desingularization Algorithm proposed in this section remains nilpotent 
with the same order of nilpotency. Moreover, when expressed in the privileged coordinates z, the 
system £ is equal to its own first order approximation in the canonical form. In other words, for any 
nilpotent systems of step k, the Desingularization Algorithm constructs a nilpotent system of step k 
and free up to step k which is in the canonical form in coordinates z. 
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3.4 Proof of Theorem 3.5 

The proof of Theorem 13.51 is based on the following proposition. 

Proposition 3.6. The desingularization algorithm is feasible from s = 1 to s = r . At each step s of 
the construction (s = 1, . . . , r), the following properties hold true: 

(Al) the vectors {£f(a s ) : I ^ are linearly independent; 

(A2) if \Ij\ < s, then ord s aS (Zj) = and ord* s (z?) = \Ij\; 

(A3) if \Ij\ > s, then ord s aS (Zj) > s; 

(A4) the change of coordinates {^j)j=i,...fi B is well defined; 

(A5) for Ik G JC S , the vector fields £f has the following form in coordinates z s , 

^(^ S )= E^ fe ^)+4(^))%+ E QtW 9 * ( 3 - 2 °) 

I 3 eH s I e eK s \W 

with ord s aS {R k ) > - \I k \, ord^ 3 (Q^) > s — \I k \, and Pf given by Eq. ( fff.Tp . 

More precisely, if one defines £| := ^i^w Pj( zS )d z a , then, one has £f = E/ e^-Pjl^)^) 

4>{j)=i 

where the polynomials P- verify the following properties: 
• if Ik EV, S , then 



for 




< \h\; 


P k = o- 




for 




= \Ik\; 


, P$ = 1, and Pf 


= 0ifk^j 


for 




> \h\: 


,ordS.(P*) = |J i | 


— \h\; 



• ifl k e K s \ W, Pf = for all j = l,...,n s . 

Remark 3.7. Property (Al) implies that Step (s-3) is feasible, which, in turn, guarantees that Steps 
(s-4)-(a) and (s-4)-(b) are well defined, and z s is a system of coordinates because the differential of the 
application y s h-> z s at is equal to the identity map. Property (A4) guarantees that Step (s-5)-(b) 
is feasible. Property (A2) ensures that, at the end of the algorithm, the system of coordinates z r is a 
system of privileged coordinates. Property (A5) finally ensures that for s = r, the approximation £ of 
£ is in canonical form. 

By Remark I3.7[ Theorem 13.51 is a consequence of Proposition 13.61 by induction on s. 

Proof of Proposition ^. b\ We begin by showing that Properties (Al)-(A5) hold true for s — 1. 
Claim 1. The family of vectors {£)(o 1 )}/eA:i is linearly independent, i.e., Property (Al) holds true for 

8 = 1. 

Proof of Claim [IJ By construction, for every I £ J , one has i]{a x ) = Xj(a), which belongs to W 1 x {0}. 
For i e Q 1 \ J 1 , the vector Q{a l ) belongs to W a x M fcl ~' Cl , and the family of vectors {£i(a 1 )}iegi\ji is 
linearly independent. Therefore, the family of vectors {CKa 1 )}/^ 1 is linearly independent and Claim 
[TJ holds true. □ 

Claim 2. For j = 1, . . . ,n 1; one has ord 1 i(zJ) = 1, i.e., Property (A2) holds true for s = 1. 
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Proof of Claim\B For j = 1, . . . , m, one has by construction £j • ^(a 1 ) = 1. Thus, one has ord a i (zj) < 
1. Since is a system of coordinates centered at a 1 , one has zj(a 1 ) = 0, which implies that ord^i^j) > 
0. Therefore, one gets ord^i(zj) = 1 and Claim [2] holds true. □ 

Claim 3. For Ij G /C 1 with \Ij\ > 1, one has ord*i(zj) > 1, i.e., Property (A3) holds true for s = 1. 

Proof of Claim\^ For | Ij\ > 2, i.e. Ij G /C 1 \ <J l , one computes ^ • zj at a 1 for every k G {1, . . . , ni}. 

ei-^V) = ^•y)(« 1 )-E(^-^)( al )(^-^ 1 )( al ) 

1=1 

= e*-»J(o 1 )-^-y i V) = o. 

Then, by definition, one has ord^(^j) > 1 for |Jj| > 1 and Claim [3] holds true. □ 

Claim 4. For i = 1, . . . , m, and j = 1, . . . , n 1; the j th — component of £j in coordinates z l is equal to 1 
= J ? an< ^ egwa/ to otherwise. In other words, for i = 1, . . . , m, the rt\ first components of verify 
Eq. A3.18\) . Properties (A4) and (A5) ZioZd true /or s — 1. 

Proof of Claim\Q By Claim [Tj ^(a 1 ), . . . , ^~ (a 1 ) is a basis of 1R™ 1 , and thus the linear change of 
coordinates i/ 1 exists. As c^i = ^(a 1 ), and zj — yj for j — 1, . . . ,ni, Claim H] holds true. □ 

Therefore, Properties (Al)-(A5) hold true for s = 1. Let 1 < s < r. Let us now assume that Properties 
(Al)-(A5) hold true for 1 < s' < s. We will show that they still hold true for s + 1. 

Claim 5. The vector fields }i=i,.„,m are well defined. Moreover, one has ord* s+ i(Pfc) = s. 
Proof of Claim\£^ Consider I k G £? s+1 \ J s+1 , then one has = [hn Ik 2 }- By Eq. f l3.6|) . one has 

Since j/fej < s and |Ifc 2 | < s, we have fci < n s and fc 2 < n s , thus the right-hand side of the above 
equation is well defined in coordinates z s = (zf, . . . , z~ ). Therefore, the new vector fields {^ +1 }i=i,..., m 
are also well defined. Since ord* a+1 (,rf Pfa) = ord* s+ i (z| 1 )+ord^ s+1 (Pc 2 ), and by inductive hypothesis 
(namely (A2) holds true at step s), one has ord* s+ i (z| ) = \I kl \, and ord* s +i(Pfc 2 ) = |ifc 2 | — 1- Therefore, 
one has ord* s+ i(P fc ) = + \I k2 \ — 1 = s. □ 

Claim 6. For Ik G IC S+1 with \I k \ < s + 1, one has 

c I : 1 (z s y +1 ) = e h (z s )+ £ £>V)V' (3 - 21) 

ijeg s+1 \J s+1 

where P^(z s ) = Pf{z s x , . . . , zQ + R^(z s ), with ord* s+1 (Pf) = \Ij\ - \I h \ and ord* s+1 (P)) > \Ij \ - \I k \. 
Proof of Claim\^ The proof goes by induction on the length \I k \. For \I k \ = 1, one has 

/ J eg s+1 \J rs+1 

0(i)=fe 

By Claim El if 0(j) = fc, then ord* s+ i(Pj) = s = \Ij\ — \Ik\- Claim [6] holds true for \I k \ = 1. 
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Assume that Claim [6] holds true for every I 6 /C s+1 of length less than or equal to s±. Consider 
h £ IC S+1 with \I k \ — S\ + 1. In coordinates (z s ,t> s+1 ), one has 

C 1 = [C'C] = ^+ E (^ 1 +A fci )V i '^ 2 + E c^+^v] 
= [ei fel ,ei fc j + E • + ^ fcl ) - c hl ■ (p? 2 + Rhn^- 

/,eg s + 1 \J s + 1 

Since (A5) holds true up to step s, one has 



(P- 2 + R- 2 ) 



E(^' 1 + ^ 1 )%+ E Of 9 * 

i 3 ew i e eic s \w 

E + ^ fcl )%^ 2 + E Qi 19 4 p i 2 + E + Rk /) 9 ^"' 2 + E #M 

I t eK s \H s I s eW heK. a \H 



k 2 



hen 



E /J ; + 



E tifd^Ft* + e (^ fcl + r^r- 2 + E 



Lew 



We first show that every term in TZ^i has, at a s+1 , an order strictly greater than s + 1 — \I k \. 

Indeed, for e H s , since ord* s+ i(^) = ord^+i (P* 2 ) = |Jj| - |4 2 |, and ord^+i (Pj 1 ) > - 

|/ fel |,one hasord^P] 1 ,^ 2 ) > - \I kl \ + - \I k2 1) - = \h\ - \I k \, with %\ = s + 1. Note that 

ord^ +1 ((P, fcl +B*)d z .B**) = ord* s+1 (P^P* 2 ). Since ord^^P/ 1 ) = |/,| - |J fcl |, and ard^flf") > 

17,1 - |4J, then, one has ord^ s+1 (Pf^P* 2 ) > - |4 2 | + - |/ fel | - \I 3 \ = \I t \ - \I k \. Recall 

that, by definition, all the functions have positive order. Therefore, one gets ord^+^Q^ 1 ^^ 2 ) > 

ord s aS+ i(Q^ 1 ) > s — \I kl \ > s + 1 — In conclusion, one gets ord* s +i (7£»,i) > s + 1 — \I k \- 
A similar computation shows that 

tL-i^ + Ri 1 ) 



E P-^Pt 1 + 



Lew 



E R^pt 1 + E ( p " 2 + 4 2 )%^ + E Qf 9 ^ 

ij£W ij&i" i e eK. s \w 

:= E P / 2 %^ fel + ^,2, with ord^ +1 (ft ij2 ) > s + 1 - |4|. 

Therefore, one gets 

C 1 = & + E {Ec^v^-^V? 1 ^ 4 - E (^i+^, 2 )^, 

with ord".+i(^,i + 7^, 2 ) > min (ord* a+1 (7^,i), ord* s+1 (7^ )2 )) > s + 1- \I k \. 

Since Corollary implies that E ( P j ld ^ P i 2 ~ P^^R? 1 ) = p t and ord^if ) = \h\ - \h 



hew 



one gets 



/<ee a + 1 \j' a + 1 
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with ord^ s +i (Pj fc ) = s + 1 — \Ik\ and ord^ s +i(i?f) > s + 1 — Therefore, Claim [6] still holds true 
for Ik G /C s+1 with \Ik\ — S\ + 1. This terminates the induction, and Claim |6] is now proved. □ 

Claim 7. The family of vectors {£,i k ^ 1 (a s+1 )}i k eic s + 1 ^ s linearly independent, i.e., Property (Al) holds 
true at Step s + 1. 

Proof of Claim\7\ Claim implies that for every I k G IC S , one has (a s+1 ) = £f fc (a s ) G M ns x {0}. 
Corollary 13.31 implies that for every 1^ G Q s+1 \ J s+1 , one has 

£/ fc + V +1 ) = Ci k {a S ) + d n e M " s x 
Therefore, by (Al) at step s, the vectors (a s+1 )} i k eK s+1 are linearly independent. □ 

Claim 8. After performing (s+l)-4-(a) and (s+l)-4-(b) in the Desingularization Algorithm, one has, 
for every Ij G H s+1 , ord^A^ 1 ) = \IA, and for every Ij G K s+1 \ W +l , ord^i^ 4 " 1 ) > s + 1. 

The proof of Claim [S] is based on the following result due to Bellaiche [21 Lemma 4.12]. 

Lemma 3.7. Let {Xi, . . . , X m } be a family vector fields defined on Q. Consider {Wi, . . . , W n } a frame 
adapted to the flag L x (a) C ■ ■ • C L r (a) =~R n at a G Q ( Remark 12.4)) . A function f is of order strictly 
greater than s at a is and only if (W" 1 • • • W% n f)(a) = 0, for all a = (a>i, . . . , a n ) such that w(at) < s. 

Proof of Claim\Bi Claim [7] guarantees that {^i +1 }i k eH s+1 * s a basis adapted to the flag L l (a s+1 ) C • - • C 
L s+1 (a s+1 ). Complete i^ 1 } i k eH s+1 by other elements of the Lie algebra generated by }i=i,...,m i n 
order to get a basis adapted to the flag L^a^ 1 ) C ■ ■ ■ C L s+1 (a s+1 ) C • • - C L r (a s+1 ). 

For Ij G IC s+1 \n s+ \ Formula (s+l)-4-(b) ensures that • ■ ■ (Z^ 1 ) P * a+1 ■ z S j +1 )(a s+1 ) = 0, for 

all P = (Pi, . . . , Pn s+ i) such, that w(P) < s + 1. By Lemma 13.71 one has ord^+x (z'j +1 ) > s + 1, for Ij G 

A v 1 \ w -. 

For Ij G H s+1 , Formula (s+l)-4-(a) implies that ((Cf+Y 1 • • • (Cj^)^ 1 • ^ +1 )( a " +1 ) = °> for a11 
P = (pi, . . . , pj-i) such that w(P) < \Ij\ — 1. Using again Lemma IBTTj one has ord*ifi(z| +1 ) > 
\Ij \ — 1, for 7j G 'H s+1 . By construction, one already has that oid s a t+ 1 (Zj +1 ) < Wj = \Ij\. Therefore, 
one finally gets ord s a t+ 1 (Zj +l ) = \Ij\, for Ij G H s+1 . Claim[8]is now proved. □ 

Claim 9. The change of coordinates (^j +1 )j=i,...,n 3+1 is well defined, i.e., Property (A4) holds true. 

Proof of Claim® After performing Steps (s+l)-4-(a) and (s+l)-4-(b), one obtains a new system of 
coordinates z s+1 . In this system of coordinates, one can write as 

3+1(2*1) = q^ +i+ J- (P M (^ +1 ) + J R M (F +1 ))^ +1 + QiAZ s+1 )d Br , 

i j en a+1 i e eic s + 1 \H s + 1 

\Ij\>2 

where P{j, Rij, and Q^t are polynomials with ord^+i (Pj) = Wj — 1, ord* + +i (Rij) > Wj, and 
ord'+itQ^) > s. Since ord*+ + 1 1 (^ +1 ) = for J, G -H s+1 , and ord'+i^ 1 ) > * + 1, for Ij G 
/C s+1 \ T-L s+1 , the polynomials Pjj contain only variables z^ +1 with Wk < Wj — 1. 

Let us now show that there exists a change of coordinates \1> S+1 which transforms coordinates z s+l 
into new coordinates z s+1 such that 

ord^zf 1 ) =wj, for I, eU s+ \ 
ord'+i^r 1 ) > s + 1, for Ij- G \ 
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and in the new coordinates, the n s+ i first components of £,i +1 (z s+1 ) are in the form 

£*+V +1 ) = 5 tm P 3 {z{ + \ . . . , z s +{) + P M (^ +1 ), j = 1, . . • , n s+1 , 
with ord^j > 5>j. 

We first note that, once one has ord*t+i = Wj for Ij G H s+1 , and ord s a t+i{z S j +1 ) > s + 1 for 
ij G /C s+1 \ H s+1 , then, the order of Py will be equal to its weighted degree, and thus automatically 
equal to m,- — 1 by construction of these polynomials. 

Consider now £j defined in coordinates z s+1 by 

Recall that, by construction, the vector fields {£i}i=i,..., m generate a free nilpotent Lie algebra of step 
s + 1. Moreover, in the canonical coordinates of the second kind . . . , ^ + x ) associated with 

}j fc ew +1 ) the vector fields are in the canonical form, i.e. 

* ■ j 

0(i)=i 

By definition of a system of coordinates, there exist n s+1 smooth functions . . . , such that, 

for j = 1, . . . , n s+ i, one has 

Expand now in Taylor series. Since ord s a f+ 1 (z s+1 ) = Wj, the Taylor expansion of is a 

polynomial of weighted degree equal to Wj. Claim is now proved. □ 

Remark 3.8. The change of coordinates )j=x,...,n s+ i is computed by identification. Indeed, since 
ord s a f+ 1 (Zj +1 ) = Wj, and the nonholonomic order does not depend on any system of coordinates, then 

is a function of order Wj at a s+1 , i.e., the Taylor expansion of at a s+1 contains only monomials 
of weighted degree equal to Wj, and there is a finite number of such monomials. Therefore, the function 

is necessarily in the following form 

E m~ z i +1 r ■ ■ ■ (^ir^^ ( 3 - 22 ) 

w(a)=Wj 

where (3° are real numbers. Eq. fl 3 . 2 2 j) is a finite sum and therefore the scalar coefficients can be 
obtained by identification. Claim [9] guarantees that such a set of real numbers ((pf) exists. Note also 
that, due to the constraint on the weight, Eq. fl 3 . 2 2 j) only involves variables z^ +1 of weight less than 
Wj, implying that the change of coordinates (^? + )j=x,...,n s +i is naturally triangular. 

Remark 3.9. Let us now illustrate Remark 13.81 with a simple example. Consider here a nilpotent 
system of step 2 generated by two vector fields (^1,^2)- We have ^ = £1, £/ 2 = £ 2 and £/ 3 = [£1,^2]- 
In coordinates z = (zi,z 2 , 23), £1 and £ 2 are necessarily in the form ^ = (1,0, a,\Z\ + a 2 z 2 ), and 
£2 = (0, 1, $\Z\ + P2Z2), where ax, a.i, 0i and (3 2 are real numbers verifying (3i — a 2 = 1. As mentioned 
in Remark 13. 8[ in the change of coordinates (^1,^/2,^3), every tyj is a homogeneous polynomial of 
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weighted degree equal to Wj. Set z = (^i(i), ^2(2), ^3(2)) =: (-21,-22,-23 + 0-21-22 + bz\ + cz%), with 
a, b, and c to be determined. One imposes that £2(2) = (0, 1, -2i). After computation, one gets 

(cui + 2ft)5i + (a 2 + a)z 2 = 0, 

(fli + a)zi + (f3 2 + 2c)z 2 = zi = z\. 

By identification, one gets a = — a 2 , b = — c=— 4f-, and in that case, /3i + a = /3i — a 2 = 1 is 
automatically verified. Then, the triangular change of coordinates 

/ s ,~ ~ ~ ~ ~ «1 ~ 2 /#2 ~2\ 

(zi, z 2 , 23) = (zi, z 2 , 23 - ol 2 z x z 2 - y^i - ) 
puts £1 and £ 2 into the canonical form. 
Claim 10. Property (A5) /io/ds true at step s + 1. 

Proof of Claim [Wi The proof goes by induction on the length of e /C s+1 . It is similar to the one of 
Claim El For \I k \ = 1, one has 

£ + V +1 ) = E + E QiAz s+1 )d z r, 

I j G'H s+1 I e £)C s + 1 \H s + 1 
4>{j)=i 

with OTd s ++x( p j) = " 1, OTd s ++i( R id) > - h OTd s ++x(Qi,e) > s - Claim [TO] holds true for |4| = 1. 

Assume that Claim [TD] holds for brackets of length less than sj. We show that it still holds true for 
brackets of length s\ + 1. Consider I k G /C s+1 with \I k \ = si + 1. Then, one has 

?4 - LSj fcl > ?/ fc2 J 

= [ E E ^V 1 ' E (^ fc2 +4 2 )V i+ E * VI 

= E [ E ^ - ^ v^i V 1 

+ E [ E + ^f 1 ) - ^ fc2 + ftw^Qj 1 
+ E of - # V iC #] V 1 - 

/££/C s + 1 \W 3 + 1 

By the inductive hypothesis, one can proceed as follows. 
• Taking into account the relation 

ord'+^itf 1 ) > \U\ - |4J, and ord^i (d z s + i ( + i?" 2 )) > |/,| - |4 2 | - |/,|, 



one deduces that 

is+l pfci a / ofe I D&2 



ffe • 



ord^itlf d^If + i?f ) > |/,| - \I k 
By a similar argument, ord s J'+ 1 R^ 2 d z s+i(Pj 1 + R* 1 ) > | Ij\ - \I k \. Therefore, one gets 

ord:+\(i^ s+1 (if + J2*») - Bfd^ilf + Rf)) > 11,1 - |4|. 
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• Since ord^i^ 1 ) = \k\-\I kl \ and ord^C^iflJ 3 ) > \Ij\-\I k2 \-\Iil then ovtf+UP^d^Rf) > 
\Ij\ — \I k \. By a similar argument, ord^i (P { d z s+i Rj ) > |Jj| — |i fc |. One thus obtains 

o^UP-'d^Rf - Ppd^iR*) > - \h\. 

• Using the fact that ord^ + 1 1 (^ + i(P/ 2 + J Rf)) > \Ij\ - \I k . 2 \ - (s + 1) and ord^i (Q* 1 ) > s + l-\I kl \, 
then oidltUQt'd^iP- 2 +Rj 2 )) > - \h\- By a similar argument, ord"+ + 1 1 Q{ 3 ^+i(P/ 1 + 
Rf) > - \I k \. One deduces oxd%{Q kl d zl+ x{P k2 + - Q k2 d z ^{P k ' + R kl )) > \Ij\ - \I k \. 

• Recall that ord^+\ (if 1 + R- 1 ) = \h\ - \I kl \, and ord^^+iQ* 2 ) > s + 1 - |i fca | - |i*|, then 
0^%{{P kl + R k ')d <+ xQf) > s + 1-|4| . By a similar argument, ord^ti ((if 2 + i?f 2 J^+iQ) 1 ) > 
s + 1 - | I k | . Therefore, it yields ord^A ( (if 1 + ) cK+i - (if 2 + R k 2 ) ^h-i Qj 1 ) > s + 1 - \I k \ . 

• Since d z s+iQ k2 is a function, one knows by definition that ord^+x (d z s+iQ k2 ) > 0. As ord^t+i (Qe 1 ) > 
a+l-\I kl \, one has ord^Q^+iQf ) > s + 1 - \I kl \ = s + 1 - (\I k \ - \I k2 \) > s + 1 - |4|. By 
a similar argument, ord^I^Q^^+iQ- 1 ) > s + 1 — \I k \. One hence derives ord s a t+ 1 (Q kl d z s+iQj 2 — 
Qfd z s + iQf) > 8 + 1- \I k \. 

Summing up the above terms, one gets, for I k E K s+1 of length s\ + 1, that the bracket ^j* 1 can be 
written in the form 

e?V +1 ) = E (^ +1 )+4(^ +1 ))<V i + E 

IJ&W+ 1 / j e/C s + 1 \« s + 1 

with ord^i (i?) = 1^1-141, ord^A (R k ) > [J,-| - |7 fc |, and ord^+i (Q k e ) > s + 1 - \I k \. Claim M is 
now proved. □ 

In conclusion, Properties (Al)-(A5) still hold true at step s + 1 in the Desingularization Algorithm. 
The induction step is established, which terminates the proof of Proposition 13.61 

□ 



4 Global Steering Method for Regular Systems 

By taking into account the Desingularization Algorithm presented in Chapter [31 we assume in this 
chapter and without loss of generality that the family of vectors fields X = {Xi, . . . ,X m } is free up 
to step r (cf. Definition I3.6p . Recall that, in that case, every point x G Q is regular and the growth 
vector is constant on Q. We present in Section H~T1 an algebraic construction of privileged coordinates 
and a nonholonomic first order approximation of X under canonical form. For regular systems, this 
construction also provides a continuously varying system of privileged coordinates. We then propose 
in Section 14.21 a global motion planning algorithm for regular systems. 

4.1 Construction of the approximate system A x 

For every point a in Q, we construct the first order approximate system A x (a) of the system A at a 
(cf. Definition 12. 14j) as follows: 
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Step (1) Take {Jfj.}j. 6 ^r. Set Wj = Wj for j = 1, . . . , n. 

Step (2) Construct the linear system of coordinates y = . . . ,y n ) such that <9 yj = XjAa). 

Step (3) Build the system of privileged coordinates z — (Si, ... , S„) by the following iterative formula: for 
j = l,...,n, 



fc=2 



where, for A; = 2, . . . , Wj — 1, 



71 ?7 

Mi/ 1 ,..., yi _ 1 ) = - £ X h---K^^y^+J2 h ^y)\y^h---^ 

\a\=k 9=2 J 

«)(a)<u;j 

with \a\ :=«! + ••• + a n . 

Step (4) For i — 1, . . . ,m, compute the Taylor expansion of Xi(z) at 0, and express every vector field as 
a sum of vector fields which are homogeneous with respect to the weighted degree defined by the 
sequence (u>j)j=i,... jn : 

X i {z)=X<f 1 \z)+X?\z)+-.-, 

where we use X t - (S) to denote the sum of all the terms of weighted degree equal to k. Set 
Xf(z) :=X\~ l) (z). 

Step (5) For j = 1, . . . , n, identify homogeneous polynomials tyj of weighted degree equal to Wj such that, 
in the system of privileged coordinates z :— {z\, . . . , z n ) defined by 

zj := ^ j (z 1 ,... : z j - 1 ), for j = l,...,n, 

the approximate system 

X%z) = {z*X?(z),...,z*X^(Z)} 

is in the canonical form. 

Step (6) Set A x (a) := X a and $ x (a, •) := the mapping x^t z. 

Remark 4.1. Steps (l)-(3) construct a system of privileged coordinates z. The proof that Sis a system 
of privileged coordinates is essentially based on Lemma 13.71 Roughly speaking, the idea to obtain Sj 
from yj goes as follows: for every a = (a±, . . . , a n ) with w(a) < Wj (so a>j — ■ ■ ■ — a n — 0), compute 
X"^ ■ ■ -Xj^ ■ yj{y)\ y =o- If it is not equal to zero, then replace yj by 



n | : <\j : : 



With that new value of yj, one gets X^ 1 •••X^ J _ i 1 • 2/j (?/) |y=o — 0. Therefore, by Lemma 15771 one 
has oTd a (zj) > Wj for j = 1, . . . ,n. On the other hand, since Step (3) of the construction does not 
modify the linear part, the system of coordinates S remains adapted. By Remark 12.51 one also has 
ord a (ij) < Wj, and therefore, ord a (Sj) = Wj. 
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Remark 4.2. The existence of tyj in Step (5) is guaranteed by a simple modification of Claim[9l page[24| 
see also Remarks 13.81 and 13.91 The key point is, in the current case, the exponential coordinates are 
algebraic. 

Remark 4.3. We will propose in Section |5] an effective and exact method for steering general nilpotent 
systems given in the canonical form. 

It results from [2] that, for regular systems, the mapping <£> x : (a,x) — > z is a continuously varying 
system of privileged coordinates on Q. Note also that the coordinates z are obtained from y by 
expressions of the form 

z\ = yi, 

z 2 = V2 + pol 2 (yi), 

z n = y n + po\ n (yi,..., y n _i), 

where, for j — 1, . . . , n, the function polj(-) is a polynomial which does not contain constant nor linear 
terms. Due to the triangular form of this change of coordinates, the inverse change of coordinates from 
z to y bears exactly the same form. Therefore, the mapping z = $ x (a, •) is defined on the whole Q, i.e., 
$ x has an infinite injectivity radius. We also note that, by construction, A x is a nonholonomic first 
order approximation (cf . Definition 12.141) and its continuity results from the continuity of the mapping 
Q x : (a, x) i — y z. In summary, we have the following proposition. 

Proposition 4.1. The mapping $ x is a continuously varying system of privileged coordinates on Q 
and the mapping A x is a continuous approximation of X on Q. 

The following theorem is a consequence of Proposition 14.11 and Corollary 12.51 

Theorem 4.2. Let V c be a compact subset ofQ. If A x is provided with a sub-optimal steering law (cf. 
Definitions 12.161 and 12. 171) . then the LAS method AppSteer associated with A x and its steering law (cf. 
Definition 12.181) is uniformly locally contractive on V c . 

Remark 4.4. Due to Step (5) in the construction procedure, the approximate system A x (a) is under 
canonical form in a system of privileged coordinates z. Therefore, A x (a) has always the same form, 
regardless of the control system X or the approximate point aGil. The specificity of each system or 
each approximate point is hidden in the change of coordinates <& x . 

Remark 4.5. It is important to notice that the approximate system used in the LAS method is 
a nonholonomic first order approximation at the goal point a (cf. Definition 12.181) . Therefore, the 
steering control always displaces A x (a) from some position (which is the image by $ x (a, •) of the 
current point of the original system) to (which is $ x (a, a) by construction) in coordinates z. The 
latter fact plays a crucial role in getting the sub-optimality for the steering law (see Section 15.31 for 
more details). 

4.2 Approximate steering algorithm 

Let V c C O be a connected compact set equal to the closure of its interior and (x imtial , x final ) G V c x V c . 
We devise, under the assumptions of Theorem I4.2[ an algorithm (Algorithm [3] below) which steers 
System (12. ip from a; imtial to s final . That algorithm does not require any a priori knowledge on the critical 
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distance £y c - Note that this algorithm bears similarities with trust-region methods in optimization (see 
[5] for more details). 

Recall first that the family of vectors fields X = {X\, . . . , X m } is assumed to be free up to step r. 
As a consequence the weights (w\, . . . , w n ) are equal at every point a G V c to (w\, . . . , w n ), the free 
weights of step r. Hence the pseudo-norm || ■ || a (see Definition I2.13P does not depend on a G V c and 
will be denoted as || ■ || r . 

The parameterized path 1 1-> S t (x) is defined by 

<J t (x) : = (t wi zx(x), . . . , f "z n (x)), for x G fi, 

where z := $ x (x final , •). Note that 5 t is the (weighted) dilatation in privileged coordinates at x final with 
parameter t. In particular, ||z(<5 t (x))|| r = \t\ ||z(a:)|| r . We also define the function Subgoal as follows. 

Subgoal(x, 77, j) 

1. i^max^l-pgL); 

2. Subgoal(x, ?7, j) := 5 t .(x) 

We note that the formula for generating tj guarantees that 

\\z(Snbgoa\(x,r),j)) - 2:(Subgoal(x, rj, j - l))|| r < 77, 
and that x d = a? 1 for j large enough. 



Algorithm 3 GlobalFree (x initial , x final , e, V c , AppSteer 



0:./: 1: 

™ „initial. tjt -.initial. 

77 := ||2;(a; mltial )|| r ; {initial choice of the maximum step size;} 
while ||z(xj)|| r > e do 

x d := Subgoal (x,rj,j); 

x := AppSteer (xi,x d ); 

if ||$ x (x d , x) \\ r > ^\\Q x (x d , Xi)\\ r then {if the system is not approaching the subgoal,} 

i] := 2; {reduce the maximum step size,} 

x := Xi\ j := 1; {change the path 5o,i(^)-} 
else 

i := i + 1; j := j + 1; 

rvi . ry> • ryd . ,-ytd . 

end if 
end while 
return Xj. 



The global convergence of Algorithm [3] is established in the following theorem. For the sake of 
clarity, we first assume that the sequences (xj)i>o and (xf)i>o constructed by Algorithm [3] both stay 
within V c . This assumption being of a purely numerical nature, we explain at the end of this section 
how we can remove it by adding suitable intermediate steps to Algorithm [3j 

Theorem 4.3. Let V c C Q be a connected compact set equal to the closure of its interior. Assume that 
(i) the approximate system system A x is provided with a sub-optimal steering law; 
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(ii) the LAS method AppSteer is associated with A x and its steering law; 

Then, V (V mtial , x final ) G V c x V c , Algorithm [3] terminates in a finite number of steps for any choice of 
the tolerance e > provided that the sequences (xj)j> and (xf)i> both belong to V c . 

Proof of Theorem \4-3\ Note first that, if the conditional statement of Line 7 is not true for every % 
greater than some io, then xf = x final after a finite number of iterations. In this case, the error ||z(a;j)|| r 
is reduced at each iteration and the algorithm stops when it becomes smaller than the given tolerance e. 
This happens in particular if d(xi, x d ) < eye for all i greater than io because condition (12. 8p is verified. 
Another preliminary remark is that, due to the continuity of the control distance and of the function 
||z(-)|| r , there exists fj > such that, for every pair (xi, X2) G V c x V c , one has 

\\z(xi) - z(x 2 )\\ r < rj =>■ d(x 1 ,x 2 ) (4.2) 

In the following, we will prove by induction that if, at some step i , one has 77 < fj, then, for all 
% > i , one has d(x i _ 1 ,xf) < (1/2 H h (l/2)'" io )e V c < £v c - 

We assume without loss of generality that io — and x = Xq. For i — 1, by construction, 
x d = Subgoal(a;o, rj, 1) and ||^(a;o) — z(x d )|| r < r/ <fj. 

In view of ( 14. 2p . one then has d(xo,x d ) < £yc/2. In view of (I2.8p . the conditional statement of Line 
7 is not true, therefore xf = x d and one has d(x ,xf) < eyc/2. 

Assume now that for i > 1 one has: 

d( Xi - 2 , xti) < (1/2 + • • ■ + (l/2) i - 1 ) £V c (4.3) 

The subgoal x d _ x is of the form Subgoal(x, 77, j). Let x d = Subgoal(x, 77, j + 1). One can write: 

d(xi-i,x d ) < d(x i -i,xf_ 1 ) + d(x d _ 1 ,x d ). 

By construction, it is ^(x^) — z(x d )||, r < rj < fj, which implies d(xf_ v x d ) < The induction 

hypothesis (14. 3p implies that d(x^x, xf_i) < |rf(xj_ 2 , xf_ 1 ). 
Finally, one gets 

d(xi- u x d ) < ^d( Xi -2, xU) + d(xU, x d ) < (1/2 + ■ • • + (1/2)%°. 

In view of (12. 8p . the conditional statement of Line 7 is not true, and so xf = x d . This ends the 
induction. 

Notice that, at some step i, rj >fj, the conditional statement of Line 7 could be false. In this case, 
7] is decreased as in Line 8. The updating law of 77 guarantees that after a finite number of iterations 
of Line 8, there holds 77 < fj. This ends the proof. □ 

When the working space Q is equal to the whole M. n , the assumption that the sequences (xj)j>o and 
{xf)i>o constructed by Algorithm [3] both stay within a compact set V c can be removed. This requires 
a simple modification of Lines 11 and 12 of Algorithm [3j 

We choose a real number R close to one, precisely (|) 1//(r+1 * ) < R < 1, where r is the maximum 
value of the degree of nonholonomy of System (I2.ip on V c . For every non-negative integer k, we set 
Rk = 1 + R + ■ ■ ■ + R h . The algorithm is modified as follows. Introduce first a new variable k, and add 
the initialization k := 0. Replace then Lines 11 and 12 of Algorithm |3] by the procedure below. 

This procedure guarantees that the sequences (xj)j>o and (xf)i>o of the algorithm both belong to 
the compact set 

K = {x <E R n : |k(x)|| r <-^-||^(x initial )|| r }. 

1 — ti 
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l: if \\z(x)\\ r > i?, fc+1 ||2(x initial )|| r then 
2: V :=f, 

3: else if i4||z(x initial )|| r < ||z(a;)|| r < R k+1 \\z(x mitial )\\ r then 
4: i:=i + l;j :=j + l; 

5. ,-yt • ry • ,-> ■ d . ,~jid . 

6: 77 := 2; 
7: fc:=fc+l; 

8: else if \\z(x)\\ r < R k \\z(x initlal )\\ r then 
9: i :— i + 1; j := j + 1; 

1 f~l . /y> . - *tti ■ rpd . rpd, . 

11: end if 



Moreover, at each iteration of the algorithm, the new variable k is such that 

II ~/ T initial\ II 

WXi)\\ r > R k \\z(x^)\\ r =► 77 < 



2* 

For the sake of clarity, we state here the complete modified algorithm named as Algorithm SJ 
Algorithm 4 GlobalFree_Modified (x initial , x final , e, V c , AppSteer) 



2 
3 
4 
■5 
6 
7 
8 
9 

10 
11 
12 
13 
14 
15 
16 
17 
18 
19 
20 
21 
22 



i : = 0; j := 1; 

, r —initial, ttt —initial. 

rj := ||2;(a; initial )|| r ; {initial choice of the maximum step size;} 
while ||z(xi)|| r > e do 

x d := Subgoal (x,i],j); 

x := AppSteer (xi,x d ); 

if ||$ x (x d ,x)|| r > Xi)\\ r then {if the system is not approaching the subgoal,} 

7] := |; {reduce the maximum step size,} 

x := xi\ j := 1; {change the path 5 0jt (x).} 
else if \\z(x)\\ r > R k+X \\z(x initial ) || r then' 

else if i4||z(x initial )|| r < \\z(x)\\ r < R k+1 ||z(x initial ) || r then 



i := i + 1; j := j + 1; 

/■>■» . - /-y» - ryd . ,->-. Q< . 

fc := A; + 1; 



else if \z{x)\\ r < R k \\z(x mitial )\\ r then 



i := z + 1; j := j + 1; 

/■>"» , - /y» - ry>d . ryd . 



end if 
end while 
return x,. 



Proposition 4.4. Le£ V c C be a connected compact set equal to the closure of its interior. Under 
the assumptions (i) and (ii) of Theorem \4-3[ V (x mitial , x final ) G V c x V c , Algorithm [H terminates in a 
finite number of iterations for any choice of the tolerance e > 0. 
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Proof of Proposition Notice that Lines 17, 18, and 19 in Algorithm H] are identical to Lines 10, 
11, and 12 in Algorithm |3j Therefore, it is enough to show that, after a finite number of iterations, 
the condition of Line 17 in Algorithm H] holds true. Another preliminary remark is that the distance 
\\z(x) —z(y) \\ r give a rough estimate of the sub-Riemannian distance. Indeed it follows from Theorem l2.3l 
that, for every pair of close enough points (x,y) G V c x V c , one has 

±\\z(x) -z{y)V l < d(x,y) < C \\z(x) - z{y)\\ l J^ +l \ (4.4) 

where Cq is a positive constant. As a consequence, Eq. (I4.2p holds true if fj < (e^ / (2Co)) r+1 . 

Let us choose a positive fj smaller than (ev c /(2Co)) r+1 . We next show that if, at some step io, r\ < fj, 
then the case of Line 10 and the one of Line 12 occur only in a finite number of iterations. Recall first 
that, from the proof of Theorem I4.3[ one gets, for every i > i , 

\\z(xf)\\ r < \\z(x io )\\ r and d(xi,xf) < e V c 

In view of Eq. (14. 4p . an obvious adaptation of the latter proof yields, for every i > i , d(xi,xf) < 
2C ^ 1/(r+1) , and thus 

\\z( Xi ) - z(xf)\\ r < (2C 2 )^ r+ % l ^\ 



Finally one gets 



\z(Xi)\\ r < \\z{xf)\\ r + \\z(Xi) -Z(xf)\\ r 

< \\z(x l0 )\\ r + (2C^ 1 W /(r+l)2 - (4-5) 



On the other hand, there exists an integer k such that 77 > ^ x 2 k jl|r ■ This implies that ||^(^j ) || r < 
i2fe ||z(x imtial )|| r . Up to reducing fj, and so increasing k , assume 

||^/^.initia^ II 



nitial^ 



since one has chosen R > (i) 1 /^ 1 ) 2 \ Using Eq. (I4.5p . it holds, for every i > i , \\z(xi)\\ r < 
i4jz(2 initial )|| r + i? fco+1 ||2(a; initial )||r = J R fco +i||z(x initial )||r- Therefore, the case of Line 10 and the one of 
Line 12 occur in at most ko + 1 iterations. Applying again the arguments of the proof of Theorem 14.31 
the conclusion follows. □ 

Remark 4.6. It is worth pointing out that the additional steps involved in Algorithm H] are designed 
to prevent the sequences (xj)i>o and (xf)i>o from accumulating toward the boundary of the compact 
V c . There exist other numerical artifacts of probabilistic nature which solve this problem. One also 
deduces from the proof of Proposition 14.41 that if the points x mitial and a; final are far enough from the 
boundary of V c , the sequences (xj)j> and (xf)i> will remain in V c . 



5 Exact Steering Method for Nilpotent Systems 

In this chapter, we devise an exact steering method for general nilpotent systems. Without loss of 
generality, we assume that the system X = {Xi, . . . ,X m } is nilpotent of step r, free up to step r, 
and given in the canonical form in coordinates x. Recall that, under this assumption, the dynamics is 
written as follows 

x,i = u u ifi = l,...,m; 

xi = ^x Il x Ir , HX I = adx I x i R i IlJr^ / H t , ^ ' ' 



33 



where the components of x are numbered by the elements of T-L r , i.e., for / 6 'H r , the component xj 
corresponds to the element Xi. We also assume that we want to steer the System (15.11) from any point 
x e R nr to the origin of K" r . 

Remark 5.1. Note that these two assumptions are not restrictive since, for general nilpotent systems, 
in order to steer from x imtml to x final , it suffices to apply the Desingularization Algorithm at the final 
point x final (see also Remark 13.61) . 

This method can also be applied for the construction of a sub-optimal steering law for the approx- 
imate system A x defined in Section 14. 1 L For practical uses, we require that the inputs give rise to 
regular trajectories (i.e., at least C 1 ), which are not too "complex" in the sense that, during the control 
process, we do not want the system to stop too many times or to make a large number of maneuvers. 

Several methods were proposed in the literature for steering nilpotent systems. In [21], the authors 
make use of piecewise constant controls and obtain smooth controls by imposing some special parame- 
terization (namely by requiring the control system to stop during the control process). In that case, the 
regularity of the inputs is recovered by using a reparameterization of the time, which cannot prevent 
in general the occurrence of cusps or corners for the corresponding trajectories. However, regularity 
of the trajectories is generally mandatory for robotic applications. Therefore, the method proposed in 
[25] is not adapted to such applications. In [21], the proposed inputs are polynomial functions in time, 
but an algebraic system must be inverted in order to access to these inputs. Moreover, the size and 
the degree of this algebraic system increase exponentially with respect to the dimension of state space, 
and there does not exist a general efficient exact method to solve it. Even the existence of solutions is 
a non trivial issue. Furthermore, the methods [21] and [25] both make use of exponential coordinates 
which are not explicit and thus require in general numerical integrations of nonlinear differential equa- 
tions. That prevents the use of these methods in an iterative scheme such as Algorithm [TJ Let us also 
mention the path approximation method by Liu and Sussmann [21], which uses unbounded sequences 
of sinusoids. Even though this method bears similar theoretical aspects with our method, it is not 
adapted from a numerical point of view to the motion planning issue since it relies on a limit process 
of highly oscillating inputs. 

5.1 Steering by sinusoids 

We consider input functions in the form of linear combinations of sinusoids with integer frequencies. 
In [22], authors used this family of inputs to control the chained- form systems. 

We first note that if every component of the input u = (u± , . . . , u m ) in Eq. (15.11) is a linear 
combination of sinusoids with integer frequencies, then the dynamics of every component in Eq. (15. ip is 
also a linear combination of sinusoids with integer frequencies, which are themselves linear combinations 
of frequencies involved in the input u. One may therefore expect to move some components during a 2-7T 
time-period without modifying others if the frequencies in u are properly chosen. Due to the triangular 
form of Eq. (15.11) . it is reasonable to expect to move the components of x one after another according 
to the order "-<" induced by the P. Hall basis. In that case, one must ensure that all the components 
already moved to their preassigned values return to the same values after each 2ir— period of control 
process, while the component under consideration arrives to its preassigned position. However, all the 
components cannot be moved independently by using sinusoids. For that purpose, we introduce the 
following notion of equivalence. 

Definition 5.1 (Equivalence). Two elements Xj and Xj in the P. Hall family are said to be equivalent 
if Ai(Xj) = Aj(Xj) for i — 1, . . . , m, where we use Aj(JTj) to denote the number of times Xi occurs in 
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Xj. We write Xj ~ Xj if X/ and Xj are equivalent and equivalence classes will be denoted by 

£ x (£ u ...,£ m ): = {Xj I A t (X f ) = l h for i = 1, . . . , m}. 

We say that the components xi and xj are equivalent if the corresponding brackets X/ and Xj are 
equivalent and equivalent classes for components are defined as follows, 

£ x (£ u ...J m ) :={xj iXjeSx^...,^)}. 

Remark 5.2. We will see in the following subsections that the frequencies occurring in the dynamics of 
xi only depend on the equivalence class of xj, and not on the structure of the bracket Xj. Therefore, the 
equivalent components (in the sense of Definition 15. ip cannot be moved separately by using sinusoids. 

Definition 5.2 (Ordering of equivalence classes). Let £ x (£i, . . . ,£ m ) and £ x (£i, . . . ,£ m ) be two equiv- 
alence classes. £ x (£\, . . . ,£ m ) is said to be smaller than £ x (£i, . . . ,£ m ) if the smallest element (in the 
sense of "-<") in £ x (£±, . . . ,£ m ) is smaller than the one in £ x (£%, . . . ,£ m ), and we write (by abuse of 
notation) £ x (£^ . . . , £ m ) -< £ x (£ u . . . , £ m ). 

Let {£ x , £ x , • • • , £ x } be the partition of the set of the components of x induced by Definition 15.11 
Assume that, for every pair G {1, . . . , N} 2 with i < j, one has £ % x -< £ x . Our control strategy 

consists in displacing these equivalence classes one after another according to the ordering "-<" by using 
sinusoidal inputs. For every j = I, . . . , N, the key point is to determine how to construct an input u J 
defined on [0, 27r] such that the two following conditions are verified: 

(CI) under the action of w- 7 , every element of £ 3 X reaches its preassigned value at t — 2n; 

(C2) under the action of nP, for every i < j, every element of £ x returns at t = 2ir to its value taken 
at t = 0. 

Once one knows how to construct an input w- 7 verifying (CI) and (C2) for every j = 1, . . . , N, it 
suffices to concatenate them to control the complete system. 

Definition 5.3 (Concatenation) . The concatenation of u 1 , . . . ,u N is defined on the interval [0, 2Nn] 
by 

u 1 * ■ ■ ■ * u N (t) := u j (t - 2(j - 1)tt), (5.2) 
for t G [2(j - l)vr, 2jvr] and j G {1, . . . , N}. 

Remark 5.3. As we will show later (see Remark l5.8p . for every positive integer k, it is possible to make 
C concatenations such that the inputs are globally of class C k and the corresponding trajectories are 
not only piecewise smooth, but also globally of class C k+1 . 

5.2 Choice of frequencies 

In this section, we fix an equivalence class £ x . We choose frequencies in w- 7 such that Conditions (CI) 
and (C2) are verified. For sake of clarity, we first treat the case m = 2 in Subsections 15.2.11 and [572721 . 
and we show, in Subsection 15.2.31 how to adapt the method to greater values of m. 
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5.2.1 A simple case: m = 2 and Card = 1 

Let xj be the only element of ££, and Xj the corresponding bracket. Let m\ := Ai(X/), and m 2 '.- 
A 2 (A 7 ). 

Proposition 5.1. Consider three positive integers u\, U3, and e G {0, 1} such that 



oo 3 = m 1 u l + (m 2 - l)u> 2 , 
£ = TOi + m 2 — 1 (mod 2) 



(5.3) 



and 

U2 > (mi + m 2 )mi. (5.4) 

By choosing properly (, the control 

7T 

ui = coswit, w 2 = cosu 2 t + C cos(aj 3 t — e— ), (5.5) 

steers, during [0,2ir], the component xj from any initial value to any preassigned final value without 
modifying any component xj, with J ~< I. Moreover, xi(2n) — xj(0) gives rise to a non zero linear 
function of (, where ( is the coefficient in front of cos(u 3 t — e~) in Eq. h5. 5\) . 

The key point is to understand the frequencies occurring in the dynamics xj. 

Lemma 5.2. For J < I, the dynamics xj is a linear combination of cosine functions of the form 

cos{(€iWi + 4^ 2 + 4^ 3 )t - {£ 3 e + 4 + 4 + 4 ~ (5.6) 

where 4,4,4 e ^ satisfy |4| < "ii? |4| + |4| < m 2- 
In particular, the term 

7T 

cos[(mio;i + (m 2 - l)w 2 - w 3 )£ - (— e + m 1 + m 2 - 1)-] 

occurs in xj with a zero coefficient depending linearly on (. 
Proof of Lemma \5.2[ The proof goes by induction on \J\. 

• I J I = 1, the result is true since ii 1 = u\ and xj 2 = tt 2 . 

• Inductive step: 

Assume that the result holds true for all J such that | J\ < s. We show that it remains true for 
J such that I J\ — s. 

By construction, we have Xj = ad^ Xj 2 with \ JA < s and |J 2 | < s. Then, 

xj 2 is given by the inductive hypothesis and xi 1 is obtained by integration of Eq. (15.61) . By using 
product formulas for cosine function, the result still holds true for J of length s. This ends the 
proof of Lemma 15.21 

□ 



36 



Proof of Proposition ^. 11 First note that integrating between and 2tt a function of the form cos(7t + 
7|) with (7,7) G N 2 almost always gives except for 7 = and 7 = (mod 2) at the same time. 
Therefore, in order to obtain a non trivial contribution for xi, xj must contain some cosine functions 
verifying the following condition 

?iwi + 4u; 2 + 4^3 = 0, , , 

? 3 £ + 4 + m 2 + 4-1 = (mod 2), 

and this condition must not be satisfied by J -< / in order to avoid a change in the component xj. 
Under conditions (15. 3 p and (15. 4p . we claim that 

(1) (mi,m 2 — 1, — l,e) is the only 4-tuple verifying (I5.8P for xi, and X/(27r) — X/(0) is a non zero 
linear function of (; 

(2) Eq. (15. 8p is never satisfied for xj with J < I. 

Indeed, consider (4,4,4) £ ^ 3 verifying |4| < ^1, |4| + |4| < m 2- One has 

4^1+4^2+4^3 = 4^i +4^2 +4((w 2 — l)u;2 + miu;i) 

= (£ 3 (m 2 -l)+4V 2 + (4 + 4m 1 )a; 1 . (5.9) 

Assume that u 2 > (mi + m 2 )miu;i. Then, except for the 4— tuple (mi, m 2 , m 3 , e) verifying Eq. (15. 3p . 
the only possibility to have the right-hand side of Eq. (15 .9p equal to is 4 = 4 = 4 = 0. In that case, 

4+4 + 4^1 (mod 2). 

Then, Eq. (15 .8p is not satisfied, and (2) is proved. 

Due to Eq. (15. 5p . the power of ( is equal to the number of times w 3 occurs in the resonance condition 
(15. 3p . The latter is clearly equal to 1. Thus, xi(2n) — £/(0) gives rise to a linear function of (. It 



remains to show that the coefficient in front of ( is not equal to zero. By Lemma [5 .2\ one knows that 

7T 

Xi = g/COs{(miWi + m 2 u 2 )t - (m 1 + m 2 - 1)-} (5.10) 

71 

+f I acos{(m 1 cu 1 + (m 2 - l)co 2 - w 3 )t - (mi + m 2 - 1 - e) — } + TZ, 

where we gathered all other terms into TZ. Note that the numerical coefficients fj and gi depend on 
the frequencies ui, u 2 , and co 3 . The goal is to show that // is not equal to zero if we want to move the 
component xj, i.e., when u 3 = (m 2 — l)w 2 + m\U)\. If we consider // as a function of ui, u 2 , and cj 3 , it 
suffices to show that this function is not identically equal to zero over the hyperplane of M 3 defined by 
the resonance condition U3 = (m 2 — l)u; 2 + rriiUi. We assume that the next lemma holds true, and we 
will provide an argument immediately after finishing the proof of Proposition 15.11 

Lemma 5.3. For all J < I , let m{ := A\(Xj) and m 2 := A 2 (Xj). // fj is the coefficient in front 
of the term co^{{m{uji + im J 2 — l)w 2 — u^t — (m{ + m^ — 1 — e)| }, and gj the one in front of the 
term cos{(m , (LOi + m J 2 uj^)t — [m[ + m J 2 — 1)|}. Then, the quotient aj := fj/gj verifies the following 
inductive formula. 

• IfXj = X 1; aj = 0; IfXj = X 2 , aj = 1; 

• If Xj = [Xj^Xjz], a j is defined by 

m^Ui + m J 2 uj 2 
m 1 1 u 1 + (m 2 - lju 2 - u 3 



where = Aj(XjJ for i — 1, 2. 



37 



Let us take U3 = —002- It results from Lemma [5.31 that, for every J < I, one has 

a j = a J± +aj 2 , if Xj — [X^ , Xj 2 ) . 

Since a.\ — and a 2 = 1, then, over the hyperplane of M. 3 defined by u 3 = — u 2 , the function aj is a 
strictly positive number independent of U\ and u 2 . 

Let us show now that aj(u±, u 2 , u) 3 ) is not identically equal to zero over the hyperplane of M. 3 defined 
by U3 = rriiUi + (m 2 — l)u 2 . Let Cj 2 '■= —m\U\jra 2 . One has m\uj\ + (m 2 — l)w 2 = — w 2 . It implies that 

oti(oj\, Cj 2 , niiuoi + (m 2 - l)£o 2 ) = cti(ui, ^2, -^2)- 

Since the function ai(u±, w 2 , — w 2 ) is never equal to zero, and it coincides with the function on(u)\, co 2 , rriiUJi+ 
(m 2 — 1)uj 2 ) at the point (coi, Cj 2 ), which is not identically equal to zero. Therefore, fi(cu\, co 2 , u 3 ) is not 
identically equal to zero over the hyperplane u 3 = (m 2 — l)u 2 + m,\U)\. Moreover, as it is a non trivial 
rational function, it eventually vanishes at a finite number of integer points. Then, we obtain a non 
zero linear function of (, and (1) is now proved. Proposition 15. II results from (1) and (2). □ 

Proof of Lemma \5. 3[ The proof goes by induction on |/|. Since x,\ = ui and x 2 = u 2 , by Eq. (15. 5p . one 
has «i = and a 2 = 1. 

Assume that \J\ > 2. By construction, one has Xj = [Xj 11 Xj 2 ] with |Ji] < \ J 2 \ < \ J\. According to 
the inductive hypothesis, one has 

x. h = gj x cos{(mf 1 u;i + m^uj^t - (mf 1 + m^ 1 - 1)^} 

+f. h cos{(mf + (mi 1 - l)u 2 - u 3 )t - (m^ + m Jl - 1 - e)~} + TZj,, 

x.j 2 = g.j 2 cos{(mf ui + mi 2 u 2 )t - (mf + m J 2 2 - 1)-} 

+fj 2 cos{(m5f 2 wi + (m J 2 2 - l)u 2 - Us)t - (mf + m J 2 2 - 1 - e) J} + ftj 2 . 
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This implies that 



7T 

xj = ( — 7 — j — g, h caa{(m 1 1 ui + m^u-^t - (mf 1 + mf)-} 

.m/wi + m 2 u 2 ^ 



m^ + (mi 1 -l)co 2 - u 3 



/ Jl acos{(m 1 1 o; 1 + (m^ - l)u 2 - u 3 )t 



-(mi 1 +m J 2 2 -e^ + Kj, 



[g j2 cos{(mf u x + mfw 2 )t - (mf + - 1)|} 
+/j 2 acos{(mi 2 a;i + (m^ 2 - 1)lo 2 - u 3 )t 



-(m{>+m J 2 *-l-e)^} + Kj 2 



7J j 1 9Jl9j2 j, cosiimfux + m J 2 u 2 )t - (m( + m J 2 - 1)-} 
^ 7712 Wi + m 2 00 2 £ 



2 Vm^cJi + m J 2 uj 2 m^ui + (m^ 1 — 1)^2 — w 3 
cos{(m^u;i + 777.2 a; 2 — u 3)t ~ ( m i + m i — 1 — + ^-J 
g.j cos{(miui + m J 2 uo 2 )t - (mi + m J 2 - 1)-} 
+/j cos{(mfwi + - u 3 )t - (mf + m J 2 - 1 - e)-} + TZj. 
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Therefore, one obtains aj 



■h , Ji 

i'I 1 (jJi + (m'2 1 —T)U}2—0J3 



"Ji + Otj 2 . 



□ 



5.2.2 A more general case: m = 2 and Card > 1 

In general, given a pair (m 1; m 2 ), the equivalence class £ x (mi,m 2 ) contains more than one element. 
This situation first occurs for Lie brackets of length 5. For instance, given the pair (3,2), one has 
both Xj = [X 2 , [X h [X h [X h X 2 \]]\ and Xj = [[X 1 ,X 2 \, [X h [X h X 2 ]]\. By Lemma E2J if one chooses 
a 4-tuple verifying the resonance condition (15. 3p for £/, the same resonance occurs in ij. Such two 
components cannot be independently steered by using resonance. The idea is to move simultaneously 
these components. For instance, one can choose (ui,u 2 ) as follows: 



Ul{t) 

u 2 (t) 



COS UJit, 

cos ui 2 t + ai cos ust + cos uj^t + a j cos cu^t, 



where ui = 1, oo 2 is chosen according to Eq. (15.4)) . Co> 3 = (m 2 — l)uj 2 + miuii, and Co> 5 = (m 2 — Yjui+miUi, 
with large enough to guarantee Condition (C2). After explicit integration of Eq. (15. ip . one obtains 



fj(uJl,U} 2 ) fj(u u u 4 ) 



Clj 



A 



a.i 



X Z (27T) - X/(0) 

x J (27r)-x J (0) 



where // and /j are two rational functions of frequencies. Thus, the pair (t7 1 ,m 2 ) controls exactly and 
simultaneously X/ and xj, provided that the matrix A is invertible. We generalize this strategy in the 
following paragraphs. Assume that ^(mi, 7772) = {xj 1 , . . . , The main result is given next. 



Proposition 5.4. Consider 



2H 



},..., {u\ 



1N1 



nil 
IN 



}■ 



mi — \ 
21 ) 



m,2 — l 



2Ni 



belonging to N miN x N m2N such that 

V j = 1 ... N. 



nil 



j W 27V 



m 2 — 1 



2iVJ 



2i - 

i=l 

£ = 777i + 777 2 — 1 (mod 2) 



E^ + E 



2j' 



1=1 



(5.11) 



and 



Vj = l...iV-l, 



11 

i+1 

Ij 
1 

2/ 

i 

2/ 
1 



N ; 



ij'+i 



1? ' 

777 

> 7722U47 1 + mi^y 

7712-1 

m 2 Lo 2 j 



> 



mi 



777i, 



2 . . . 771 2 - 1 , 



(5.12) 



T/jen, t/ie znpiti 77 := (t7i, 77 2 ) defined by 



«2 



TV mi 

EE 

i=i »=i 

TV m 2 -l 



COS Wyt, 



(5.13) 



7I\ 



cosw^t + cos(a;^7; — £— 



=1 i=i 



steers the components (x^, . . . , xi N ) from an arbitrary initial condition (2^(0), . . . , xi N (0)) to an arbi- 
trary final one (xi 1 (2n), . . . ,xi n (2tt)), without modifying any other component having been previously 
moved to its final value. 
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This result generalizes Proposition 15.11 The proof is decomposed in two parts as follows: 

Part I: we show that, if (I5.12p holds and the control functions are of the form (I5.13p . then ( 15. lip is the 
only resonance occurring in (xj 17 . . . , Xj N ); 

Part II: as the resonance gives rise to a system of linear equations on (a 1; . . . , a^), we recover the invert- 
ibility of this system by choosing suitable frequencies in the control function (I5.13p . 

Part I Frequencies and Resonance 

Consider inputs of the form (I5.13p . Generalizing Lemma [5 .2\ we give a general form of frequencies 
involved in xj. 

Lemma 5.5. The dynamics xj is a linear combination of cosine functions of the form 

{h ■oo 1 + £ 2 -oo 2 + t 2 - oo* 2 )t -(£ 1 +£ 2 + m* 2 -l + t 2 e)^ (5.14) 



where 



N mi N m 2 -l N 

* - = E E W ■ <* = E E Vi. ^-^ = E 

j=i i=i j=i i=i j=i 

N mi N mi— 1 N 

^ -EE^- ^ = EE % % = E^-, 

j = l j=l j=l f=l j = l 

uritt (4,-,4j^2i)eZ 3 . 
Lei 

N mi N m 2 -l TV 

N = E5>U N = E E i*U 141 = Ei^'i' 

j=l i=l j=l i=l j=l 

then, one has [i^ < A^Xj), \£ 2 \ + \£*\ < A 2 (Xj). 
Proof of Lemma \5. 51 The proof goes by induction on \ J\. 

• | J\ — 1: the result is true since x,\ = U\ and x 2 = u 2 . 

• Inductive step: 

Assume that the result holds true for all xj such that 1 < | J\ < s. We show that it remains true 
for x,j with | J\ = s. By construction, we have Xj = ad^ Xj 2 , and 

with |Ji| < |J|, \J 2 \ < | J|, and k\J\\ + \J 2 \ = \ J\. 
Then, by the inductive hypothesis, we have 

xj, = UnCom {cos^ ■ Ui + £ 2 ■ uj 2 + t 2 ■ u;)t £ 2 + t 2 - I + eie)^}} , (5.16) 
x j2 = LinComjcosl^-Wi+^-^ + ^-w^t-^i+^ + ^-l + ^e)^}}, (5.17) 

where LinCom{-} stands "linear combination". 
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Eq. (15.161) implies that 

x Jx = LinCom|cos{(£i-o; 1 + £ 2 -W2 + ^-^)t-(£i+£2 + ^2- 1 +^)^-^}} 

= LinCom|cos{(£i-o;i + £ 2 ■ u 2 + 1 2 ■ u^)t - (4 + 4 + ^2 + ^)|}} • (5-18) 

For notational ease, we will only write down the case xj = xj 1 xj 2 . Using product formulas for 
cosine function, one has 

xj = LinCom (cos{[(4 ± 4) ■ u x + (4 ± 4) ■ u 2 + [t 2 ± t 2 ) ■ u* 2 }t 

-[{I, ± 4) + (4 ± 4) + (t 2 ± t 2 ) -l + (t 2 ± §)e]|}} . (5.19) 

Moreover, according to the inductive hypothesis, one has 

\£ 1 \<a 1 (x Ji ), \e 2 \ + \e* 2 \<A 2 (x Jl ), 

and 

|4| < A 1 (X J2 ), |4| + \1* 2 \ < A 2 (X J2 ). 

Then, one gets 

|4 ± 4| < A^Xj), and |4 ± 4| + |«5 ± ^| < A 2 (Xj). 
This concludes the proof of Lemma 15.51 

□ 



By Lemma I5.5[ one gets a non trivial contribution for x j if the resonance condition 

?i • wi + 4 • 0J2 + 1 2 ■ uj* 2 = 0, 
^e + 4+4 + ^2- ! = (mod 2), 



(5.20) 



is verified by the frequencies of some cosine functions involved in xj. 

Lemma 5.6. Under conditions (15. lip and (I5.12p in Proposition 5.4, one gets a non trivial contribution 
on X/. depending linearly on aj for all j = 1 . . . , N . 

Proof of Lemma 15.61 It is clear that the resonance condition (15.201) holds for 

{u n , . . . . . . , {w\ N , ■ • • 

{ui 2 l, • • • , UJ 2 i , W 21 }, . . . , {w 2 7V) • • • > ^2N ' UJ 2Ni~i 

and e G {0,1} verifying (15. lip . We show that it is the only resonance occurring in xi y Indeed, by 
Lemma 15. 5[ the integer part of frequencies in xi. is in the following form 

4 • OJl + 4 • <^2 + £ 2 • ^2 
N mi N m 2 -l N 



E E 4<4 + E E + E 



j = l 8=1 j = l j=l j = l 



A 1 " mi ./V m2-l A 1 " / mi m 2 — 1 

2j 



E E 4<4 + E E 4-4 + E 4 E + E 

A 1 " mi N m,2 — 1 

E E(4 + 4K + E E (4 + 4K- (5.2i) 

j'=l t=l j'=l i=l 
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By Condition (15.121) . Eq. (I5.2ip is equal to zero if and only if 



-lj+^j = 0, for % = l,...,mi, 
?2j+%j = °> for i = l,...,m 2 - 1. 



Then, one has 



'-2j I > 



N mi N mi N 

n = EEfei = EEi^i = ^Ei^ 
j=i i=i j=i i=i j=i 

AT m 2 — 1 ma— 1 

n = E E 14-1 = E E i^i = K - 1) E i^-i- 

j=l i=l j=l »=1 j=l 



However, by Lemma |5.5[ one knows that < m x and |£ 2 | + |^ 2 | — m 2- Then, one necessarily has 
m* 2 - = for all j = 1, ... , iV. In that case, one obtains 

^£ + 4+^2 + ^-1 = -1^0 (mod 2). 

In conclusion, the resonance condition ( I5.20p does not hold for any 4— tuple (£i,£ 2 ,£ 2 ,s) different from 
(m 1; m 2 — 1, —1, rrii + m 2 — 1 (mod 2)). 

By Eq. (]5.13j) . the power of a,j is equal to the number of times u 2 j occurs in the resonance condition 
( 15. 3p . Since the latter is equal to 1, we obtain a linear function of aj. This ends the proof of Lemma 
1531 □ 

Lemma 5.7. If xj G E l x and i < j, then xj(2n) — xj(2ti) = 0. 

Proof of Lemma \5. 7[ We first note that Eq. (I5.2ip still holds true. Recall its expression here. 

N mi N m 2 -l 

i x ■ oui + £ 2 ■ u 2 + t 2 ■ u* 2 = Efe + £ 2j) ■ *>v + E E (4 + ■ ^3 ( 5 - 22 ) 

j=l i=l j=l i=l 

By condition (15.121) in Proposition 15. 4[ Eq. ( I5.22p is equal to zero if and only if £\j + £ 2 j = f° r 
i = 1,. . ., mi, j = 1, . . . , N and t 2 j + £ 2 j = for i = 1, . . . , m 2 — 1, j = 1, . . . , N. In that case, one has 

N N 

|4| = miEl^l> l^| + |^|=m 2 ^|^.|. 

One also knows that \£i\ < A x (Xj), |£ 2 | + |^l < A 2 (Xj), with A x (Zj) < mi or A 2 (Xj) < m 2 . 
Therefore, one has £ 2j = for all j — 1, . . . , N. This implies that 

t 2 e + £ x + £ 2 + £* 2 ~ 1 = -1 ^ (mod 2). 

In conclusion, the resonance condition (I5.20p does not hold true. This ends the proof of Lemma 
EH □ 

Part II Invertibility 
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As a consequence of Lemma I5.6[ one has 

/ x h {2n) - x h {0) \ 

\x lN (2n) - x lN (0) J 



m.2 — 1 * 



ft 



hi^li, ■ ■ ■ , ^2i), 



) .// V (^'|'.V- ■ ■ ■ ■ ^2 Y 



Jjv^ll' • • • i U 2l)i 

where fj. : M. m — > K. are rational functions of frequencies, and every u)%j verifies Eq. (15.111) for 

./ 1......V. 

Definition 5.4 (Control matrix and control vector). The matrix A and the vector (ai,...,ajv) oc- 
curring in Eq. ( I5.23|) are called respectively control matrix and control vector associated with the 
equivalence class £ 3 X . 

We show in the sequel that it is possible to choose integer frequencies 

{L0 U , . . . ,COii}> •••■> WlNi ■ ■ ■ j u Tn}i 



f CLl\ 

\o>n) 



(5.23) 



)) ••• > fl N (v 1N , • • • ' U 2n)J \ a N/ 



{u\ x , . . . 



■> UJ 21 



1712 — 1 ,* 



, a>2i}, • • • , {^2 



2Ni • • • ' UJ 2N 



7712 — 1 ,* 



) U} 2NSl 



so that the invertibility of the control matrix A is guaranteed, as well as the non-resonance of every 
component xj belonging to a class smaller than 8^. 

For j = 1, . . . , N, we use Pj to denote the hyperplane in M. M with M := mi + m 2 defined by Eq. 
(15. lip , for which we recall the expression next, 



(Hi 



7712 — 1 



UJ. 



2j 



UJ. 



2r 



1=1 



1=1 



We start by showing that the function detA(cuJ 1 , . . . , oj^n) ls no ^ identically equal to zero on n j=1 P 7 -. 
This is a consequence of the following lemma. 

Lemma 5.8. The family of functions 

. . . ,< 2 " 1 ,^), • • • , ffMi ■ ■ -^2 2 ' 1 M)} 

is linearly independent on the hyperplane P in R M defined by the equation 

7Hl 7712 — 1 



£-:+£ 



1=1 



1=1 



Proof of Lemma \5.8l The first part of the argument consists in considering a family of M indetermi- 
nates Y = {Yi, . . . , Ym} and the associated control system 



M 



y = YL ViYi - 



(5.24) 



i=i 
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Let Hy be a P. Hall family over Y . Consider the elements {Yj lf ■ ■ ■ , Yj. } in iify of length M such that 
Ai(Yj.) — 1, for i = 1 . . . M, and j = 1, . . . , N, and the corresponding components {yj 1 , ■ ■ ■ , in 
exponential coordinates. 

If we apply one control of the form {vi = cos z^}i=i...M, with z/ m = v ^ ^° System f)5.24p . then, 

by explicit integration, there exists, for each component yj ., a fractional function /J : M m — >■ R such 
that 

A/-1 

^(27r) - yj.(O) = fj.(vx, . . . , z/ M ), for z/ M = v t . (5.25) 
Claim 11. The family of functions {fj x , ■ ■ ■ , /jC} linearly independent on the hyperplane in M. M 



M-l 



defined by v M = J2i=i 



Vi. 



Proof of Claim Q7J We first define fj by 

fj.(vi, ...,u M ) = fj^i, . . . , -v M )- (5.26) 
Then, it is easy to see that fj. verifies the following inductive formula: 

1. for J = i = l...M,fJ(v i ) = ±; 

2. for | J | > 1, Yj — Yj 2 ], there exists an injective function 

aj:{l,...,m J }^{l,...,M} 

such that 

7y i \ _ fJii^ji 1 )' ■ ■ ■ ' U (Tj(m J i)) 7y I \ 
J J l^j(l)> • • • > V <Tj{m J )) - — -pi Jj 2 (ycrj(m J i+l)) ■ ■ ■ ,V<rj(m J ))> [O.Zf) 

where m J : = A(Yj), m Jl := A(Y Jl ), and m j2 : = A(Yj 2 ). 

We note that the family of rational functions fj is well defined for all the Lie brackets Yj such that 
Aj(lj) < 1, i — 1, . . . , M. The algebraic construction could be extended to all the Lie brackets, but it 
is not necessary for our purpose. We also note that Claim [11] is equivalent to the fact that the family 
of rational functions 

• • • , ^M)} i=W v 

is linearly independent over the hyperplane Y^iLi v % = 0- 

Recall that every element Yj j in the family {Yj x , . . . , Yj. } writes uniquely as 

Y Jo = [Y Jn ,Y Jj2 ]. (5.28) 

Definition 5.5 (Left and right factors) . For J G {Ji, . . . , J^}' the left factor L(J) and the right factor 
R(J) of J are defined in such a way that Yj = [Yl(j), Yr(j)]- 

Let L* be defined by 

L* := max (5.29) 

i=l,...,iV 

The integer L* is well defined since a P. Hall family is totally ordered. Thus, there exists J* G 
{Ji, . . . , J^} such that L* = L(J*). Then, define .R* := R(J*) and set m* = \L*\. Let 

A = A L U A R and A = {1, . . . , N} \ A, 
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with Al and Ar defined by 

A L := {j e{l,...,N}, such that Y L{Jj) ~ Y L }, (5.30) 
Afl := {j e{l,...,N}, such that Y L(Jj) ~ (5.31) 
Then, for all j G A, there exists an injection function 

^:{1,...,M}^{1,...,M} 



such that one has 



/L(J 3 ) K(l)r-- ) ^ (m')) B 

Em* 



fj i (u l ,...,u M ) = f R{J . ) (z/ CTj(m * +i),..., ^.(M)), ifjGAi, (5.32) 



FY, v /i(j.)K 3 (m*+l),---,^(M)) ~ y . 

f J .{v 1 ,...,p M ) = — ^ /^^(^(i),...,^*)), ifjeAfl. (5.33) 

Z^i=m* + 1 ^OjW 

Note that, for all j 1 and j 2 in Al, one has {^(l), • • • , v<r h (m*)} = {^(i), • • • , ^ 2 (m*)}- Denote by 
the set of variables involved in /£(j.) with j G A^. A similar property holds for A#. For all j\ 
and j 2 in A R , one has {^(r^+i), • • • , ^(Af)} = {^ 2 (m*+i), • • • , z%(Af)}- Denote by EE^ the set of all 
variables occurring in jf^j.) with j G A#. Then one has S L U S fl = {z^, . . . , z^f}- By abuse of notation, 
we re-write Eqs. ( I5.32p and (I5.33P in the following form: 



cY 



v M ) = £ J> ~ flu^R), ifjeA L ; (5.34) 

/>!,■■,%) = ^ (Jj) ^ /I (Jj) (5l), ifj6A fl . (5.35) 
Moreover, by the resonance condition J^fLi v % — 0, Eq. (15.351) becomes 

ff i ("i,...,VM)= f ^ R l /I (Jj) (5l), ifiGA a . (5.36) 

We now prove that the family of rational functions • • • , ^m)}j = i tv i s linearly independent 

over the hyperplane YliLi v i = ^- The proof goes by induction over the length of the Lie brackets under 
consideration. For the brackets of length 1, the result is obviously true. Assume that the result holds 
for all brackets of length smaller than M — 1, M > 2. 

Assume that there exist Ej G ~R N such that 



N M 

J2^IJM,---^m) = 0, withj> f = 0. (5.37) 

j=l i=l 



One has 



N 



J2zJJM> • • • ' ^) = 5>/£(**> • • • ' + E • • • > ( 5 - 38 ) 

3=1 ieA jeA 
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Multiplying Eq. (I5.38|) by the factor E] vj., one gets 



E ^fliW^L)f Y R{J ^ R ) - E ^)(Sh)^j,)(Sl) + ( E ^EVl^i. ■■■^m) = 0. (5.39) 

jeAz, ieA H vu&l jeA 

Since L* is the maximal element among the left factors of Lie brackets of length M, the fraction fj. 



does not contain the factor for all j G A. Therefore, on the hyperplane of M m * defined by 

E] ^fc = 0, one has 

E WliJ^fl^R) - E ^/L y (j J )(^)/I (Jj) (S L ) = 0. (5.40) 
Fixing variables belonging to Sr, Eq. (15.401) is a linear combination of elements of the family 

{fliJ^i-L^jeAL U {/j i (J J .)(Sl,)}i6A J j 

associated with elements of length m* in the P. Hall family. By the inductive hypothesis, this family 
is linearly independent over the hyperplane of M m * defined by = 0. We therefore obtain that 



ejf^j^En) = 0, for all j G A L , (5.41) 

^ (J .)(S«) = 0, for all jG A*. (5.42) 

Since Eqs. (15.411) and (15.421) hold true over the whole hyperplane of K M ~ m * defined by = 0, 
one has Ej = for every j G A. Therefore, Eq. (15.381) becomes 



^£4K..,, M ) = 0. (5.43) 

jeA 

Consider now the maximum left factor for j G A and iterate the same reasoning used for Eq. (15.371) . 
We deduce that £j = for every j G A. Therefore, the family {fj.(vi, ■ ■ ■ , v m)} j^...^ is linearly 
independent over the hyperplane Y^i=i v i = ^ an d this concludes the proof of Claim [TTJ □ 

We are now in a position to proceed with the argument of Lemma 15.81 Let Xj be an element of 
8 x{ttii, 1712)1 M := mi + m 2 and N :=Card Sximi, ^2)- Consider also another family of M indetermi- 
nates Y = {Yi, . . . , Ym} and let Hy be the P. Hall family over Y. Finally, consider all the elements of 
the class £ Y (l, . . . , 1) = {Y Jl} Y Jfj ] in H Y . 

Let LT be the algebra homomorphism from L(Y) to L(X) defined by 

Il(Y i ) = X 1 , for z = l,... s mi, (5.44) 
Yl(Yi) = X 2 , for % = mi + 1, . . . , M. (5.45) 

Note that the map LT is surjective from £ Y onto Ex- Consider the following vector fields 

V Y = {v x Y l + ■■■ + v M Y M }, 
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where 

7T 

Vi = cos u{t, for i = 1 ... M — 1, and vm = cos^^i + s— ), (5.46) 

M-\ 

with ujm = Wi, and verifying the non- resonance conditions. 

i=i 

Then, the non-autonomous flow of Vy between and 27r is given by 

ej$(V Y )(0, 2ir) = o - - • o e 7 ^* JJ (5.47) 

j>Ji 

Let us now apply II to Vy, we get 

n(W) := V x = {vxnCn) + ■ ■ • + tvJI(F m )} = {u 1 X 1 + u 2 X 2 }, (5.48) 

where 

mi mi 

U\ = v i = cos uj{t, (5.49) 

i=l i=l 

m m— 1 __ 

k 2 = f j = coswjt + cos(a; m t + e— ). (5.50) 

i=m\ + l i=mi+l 

Then, the non-autonomous flow of Vx between and 2n is given by 

e^(V^)(0,2vr) = e / "i n( ^ ) o---oe / ^ n(yj - ) o J[ e fJ^) = e ^ f l n ^ Q J[ e^ n ^\ (5.51) 

j>Ji j>Ji 

We also know that 

/>/i />/i 



ffl^(Vx)(0, 2vr) = o ■ ■ ■ o e ^ x ^ J] e /f = e ^% Xl i J] e^ x '. (5.52) 



Recall that II is surjective from £y(l, . . . , 1) onto £x{mi,m2)- Therefore, by identifying Eqs. f)5.5ip 
and (I5.52p . we obtain that, for all j = 1, . . . , N, f x is a linear combination of fj, with i = 1, . . . , N, 
i.e., 

N 

Since the family (/^y j is linearly independent and the matrix A := (of ) i=1 ... jvy=i ...,n * s surjective, 
we conclude that the family (f x )j=i,...,N is also linearly independent. This ends the proof of Lemma 
15781 □ 



A consequence of Lemma 15.81 is the following. 
Corollary 5.9. With the above notations, the function detA is not identically equal to zero on C\^ =1 Pj. 
Proof of Corollary 15. PI For j = 1, . . . , N, we define the vector Lj by 

Lj = [J* (^11, • • • , ^21 \ W 2l)) " ' ) fhji^lNi ■■■■> ^ZAT X > W 2A^ 
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N 

Assume that ijLj 
3=1 



with L e R. Then, for alH = 1, 



N, we have 



AT 



W li) • • • ) W H )^2i) 



. IjJ. 



m 2 — 1 
2i : 



2i> 



0. 



(5.54) 



By Lemma [5.81 we have lj = for j — 1, . . . , N. Then, the family (Lj)j=x n is linearly independent. 
We conclude that det A is not equal to zero. This ends the proof of Corollary 15.91 □ 

We still need another technical lemma which guarantees that there exist integer frequencies such 
that Eq. ( I5.12p is satisfied and the matrix A in Eq. (I5.23P is invertible. 

Lemma 5.10. There exists integer frequencies such that (I5.12p is satisfied and det A is not equal to 
zero. 

Proof of Lemma \5.1(A For j = 1, . . . , N, we set 



then, we have 



m— 1 



fj(Ul, 



-l) — flji^li ■ ■ ■ 5 Ui), 



i=l 



(5.55) 



detA 



fM 



Hi 



/jv(^ih 



7712 — 1 s 



) W 21 



7712 — 1 "> 
21 



1JV> 



lAf) • 



) ^2^ 



J7l2-1\ 



7)72 — 1^ 

2AT / 



. UJ. 



7772 — 1 N 
2JV > 



) W 2V J 



(5.56) 



where P and Q are two polynomials of (m — 1)N variables. 

We first note that Q never vanishes over integer frequencies. Assume, by contradiction, that P is 
always equal to zero for integer frequencies verifying Eq. f)5.12p . Consider P as a polynomial in one 
variable cj^/v" 1 , i.e., 



M 



P(ul 



11) 



7772-1^ 

) UJ 2N , 



3=0 



11) 



) UJ 2N 



2N 



(5.57) 



Given integer frequencies (oj\i, ■ ■ ■ , ui^~ 2 ), if Eq. (I5.57P is not identically equal to zero, then this 
polynomial in the variable w^r -1 most has a finite number of roots. However, for a given choice of 
(m — 1)N — 1 first frequencies, there exist an infinite number of verifying (I5.12p . Then, Pj = 

over all integer frequencies, and Pm is not identically equal to zero. We note that all Pj are polynomials 
of (m — 1)N — 1 variables. Proceeding by induction on the number of variables, it is easy to see that, 
at the end, we obtain a polynomial in the variable which is equal to zero over all integer u)\ x , and 
which is not identically equal to zero according to Corollary 15.91 That contradiction ends the proof of 
Lemma 15.101 □ 



5.2.3 General case: m > 2 

Notice that the proof of Theorem 15.41 does not really depend on the number of vector fields involved in 
the control system (12.11) . Indeed, for m > 2, if the control functions are linear combination of sinusoids 
with integer frequencies, then the state variables in the canonical form are also linear combinations of 
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sinusoids so that the frequencies are Z— linear combinations of the frequencies occurring in the control 
functions. The proof is the same as that of Lemma 1531 up to extra notation. Since Lemma fTTl depends 
only on the length of the Lie brackets, but not on the number of vector fields, the proof of Lemma 15.8 
does not depend on m, either. In order to prove a similar result for m > 2, we just need to re-project 
Eqs. (15.44ft and (15.45ft to m vector fields instead of 2. 



5.3 Exact and sub-optimal steering law 

In this section, we explain how we can devise, from Proposition 15. 4[ an exact and sub-optimal steering 
law (cf. Definition 12.17ft Exact mjJ . for the approximate system, which is already in the canonical form 
and how Exact mjr can be incorporated into the global approximate steering algorithm (cf. Section |4~2]) . 
Note that Exact mjr only depends on the number of controlled vector fields m and on the maximum 
degree of nonholonomy r. 

Recall that the components of x G M, nr are partitioned into equivalence classes {£%,£%,... ,££} 
according to Definition 15.11 in such a way that £1 -< £{ for (i,j) G {1, • • • , N} 2 and i < j. For every 
equivalence class ££, Proposition 15.41 and Subsection 15.2.31 guarantee that we can choose frequencies 
such that the corresponding control matrix Aj (cf. Definition 15.4ft is invertible and the corresponding 
control function obtained by Eq. (15. 13ft steers all the elements of £ ] x from an arbitrary initial value 
to the origin (see Remark 14.5ft without modifying any elements belonging to smaller classes. 

Let x initial G M" r . Let Bj := Aj 1 and Nj := Card(fij), j = 1,...,N. For x G , we will use 
Fk...,fc with 1 < i < k < n r to denote the vector (xj, . . . ,Xk), and ||x|| to denote the pseudo-norm of 
x defined by the free weights (cf. Definition 12.131 and Definition 13.7ft . We also define an intermediate 
function Position(w) as follows: if System (15.1ft starts from x = and evolves under the action of u, 
then Position(w) returns its position at t = 2n. 

Algorithm 5 Exact Steering Law: Exact mjr (x imtial ) 
Require: B\, . . . , B^, and Ni, . . . , iV^; 



1 


\ . ||™initial|| . 

A ■— If 1 1 o ? 


2 


™new . j: /™initial\. 


3 


^norm • 0, 


4 


i:=0; 


5 


for j = 1, . . . , N do 


6 


X := [x new ]i4-i ) ... ) i4.jv ;) -; 


7 


a J := Bj x; 


8 


construct m- 7 from a- 7 bv Eq. (15.13ft; 


9 


x new := x new + Position (u j ); 


10 


Unarm '■= u norra * v? (cf. Definition 15.3ft 


11 


% = i + Nj; 


12 


end for 


13 


return u := AM norm . 



Proposition 5.11. For every x imtial G M. nr , the input given by 

Exact mjr (x imtial ) steers System A5.1\) from x imtial to exactly. Moreover, there exists a constant C > 
such that 

^(Exact mjr (x initia1 )) < Crf(a; initial , 0), V x initial G M" r , (5.58) 
where we use d to denote the sub-Riemannian distance defined by the family X . 
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Proof of Proposition [37771 The fact that the procedure described by the Lines 5 — 12 in Algorithm |5] 
produces an input M norm steering System ( 15 .ip from 5 i(x mitml ) to is a consequence of Proposition 
15.41 and Subsection 15.2.31 We also note that, due to the homogeneity of System (15.11) . if an input u 
steers (15. ip from x to 0, then, for every A > 0, the input Am steers (15. ip from 5o t \(x) to 0. Therefore, 
the input computed by Exact mjr .(x imtial ) steers System (15.11) from a; imtial to 0. Let us now show (15.581) . 
In the sequel, the application Exact m;r . : x — > Exact mjr (x) will be simply denoted by u : x — > u(x). Let 
.5(0, 1) := {y, 1 1 2/ 1 1 o = 1} an d x G M" r . Then, there exists x norm G S(0, 1) such that x = S 0! \(x noTm ) with 
A := ||x||o- We have: 

£(u(x)) = £(\u(x norm )) = \£(it(x norm )) < A sup £{u{y)). 

yeS(0,l) 

We also know that, since the sub-Riemannian distance d(0, •) from and the pseudo-norm || • ||o at 
are both homogeneous of degree 1 with respect to the dilation 5o,t{-), there exists a constant C > 
such that CX < d(0,x). Since the application y — > u(y) is continuous from 5(0, 1) to M. m and S(0, 1) 
is compact, then, sup y( zs(o,i) £{u{y)) * s bounded, thus the inequality (I5.58P holds true. □ 

The following theorem is a consequence of Proposition 15.111 and Remark 14.41 

Theorem 5.12. The function Exact mjr (-) constructed by Algorithm ^provides the approximate system 
A x defined in Section J^.l with a sub-optimal steering law. 



Proof of Theorem \5.1 6 A It suffices to note that, for every affl, A x (a) has the same form (cf. Remark 
14. 4p . thus defines the same sub-Riemannian distance d. Therefore, the inequality ( I5.58P holds uniformly 
with respect to the approximate point a, and this terminates the proof of Theorem 15.121 □ 

Remark 5.4. Frequencies choices and the construction of the corresponding control matrix Aj, as 
well as its inverse Bj, translate to off-line computations. We note that Proposition 15.41 only gives 
sufficient conditions to prevent resonance (by choosing widely spaced frequencies, cf. Eq. ( I5.12p ) 
and guarantee the invertibility of the corresponding matrix (by using a sufficiently large number of 
independent frequencies). These conditions tend to produce high frequencies while it is desirable to 
find smaller ones for practical use. We can prove that two independent frequencies suffice to steer 
one component (cf. Section I5.2.ip . and we conjecture that 2N independent frequencies suffice to 
control one equivalence class of cardinal N by producing an invertible matrix. One can implement a 
searching algorithm for finding the optimal frequencies for each equivalence class such that they prevent 
all resonances in smaller classes and produce an invertible matrix for the class under consideration. 
Proposition 15.41 guarantees the finiteness of such an algorithm. Moreover, one can construct, once for 
all, a table containing the choice of frequencies and the corresponding matrices for each equivalence 
class in the free canonical system. 

Remark 5.5. Recall that the key point in our control strategy consists in choosing suitable frequen- 
cies such that, during each 2n— period, the corresponding input function displaces components of one 
equivalence class to their preassigned positions while all the components of smaller classes (according 
to the ordering in Definition 15. 2p return at the end of this control period to the values taken at the 
beginning of the period. In order to achieve the previous task, special resonance conditions must be 
verified by the appropriate components, and these conditions must not hold for all the other smaller 
components (according to the ordering in Definition 15. 2p . Note that two categories of frequencies 
have been picked in Proposition 15.41 the basic frequencies {oofj}, and the resonance frequencies {oo*j}- 
Since frequencies occurring in the dynamics of the state variables are just Z— linear combinations of 
{oJij} U {uj*j}, and the resonance frequencies {uj*j} are chosen to be special Z— linear combinations 
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of basic frequencies (resonance condition), the frequencies in the dynamics of the state variables are 
special Z— linear combinations of {ujfj}- 

Remark 5.6. Once the frequencies and matrices are obtained, the on-line computation Exact mjr is 
only a series of matrix multiplications which can be performed on-line without any numerical difficulty 

Remark 5.7. The Desingularization Algorithm presented in Section 1331 (see also Remarks 13.61 and 15 . 1 j) 
together with Algorithm [5] provides general nilpotent control systems with an exact steering method, 
which is also sub-optimal. 

Remark 5.8. We note that the inputs constructed in this section are piecewise C°° during each time 
interval [2iir, 2(i + l)ir], for i = 1, . . . , N — 1, but they are not globally continuous during the entire 
control period [0, 2Nir], due to discontinuity at t — 2ir, Air, . . . , 2(N — However, it is not difficult 
to devise (globally) continuous inputs using interpolation techniques. We illustrate the idea with a 
simple example. Assume that we use u % = {u\,u l 2 ) and u 3 = {u{,u 2 ) defined by 

u\(t) = cos U) ut, 

. .7T 

u 2v) = cos u 2it + a 1 cos((X>2/£ + e l — ), t e [2(i — l)ir, 2m\, 

u\{t) = COSUijt, 

■ IT 

u i(t) = cosoj 2 jt + a 3 cos(a;2,t + s° — ), t E [2{j — l)ir, 2jir], 

J 2 

to steer two consecutive classes £ l x and £{ (i.e. j = i + 1) which are both of cardinal equal to 1. 
If we require their concatenation u 1 * u 3 to be continuous, i.e. 

wi(2vr) = u{{2tt), (5.59) 
4(2tt) = ui(2n), (5.60) 

we can proceed as follows. 

For Eq. f !5.59j> . it suffices to modify slightly u\. We take 

u{(t) = u\(2ir) cosuijt. (5.61) 

For Eq. f l5.60j> . we distinguish two cases: 

• if e J ' = 1, we can take 

u J 2 (t) = u 2 (2ir) cos u 2 jt + a 3 cos(u 2j t — — ) 

J 2 

= u 2 (2tt) cos u 2 jt + a J sin cu 2 jt; (5.62) 

• if e J = 0, we add a frequency u c to u 2 , large enough to avoid any additional resonances, 

u 2 (t) = cosu 2 jt + a? cosu 2 jt + (u 2 (2n) — a? — 1) cosa; c t. (5.63) 

Let u 3 := (u{, u 2 ). Then, by construction, the new input u l * u 3 is continuous over the time interval 

[2Z7T,2J7T]. 

It is clear that this idea of interpolation by adding suitable frequencies can be used to construct 
continuous inputs over the entire control period [0, 2N%\. In fact, by using more refined interpolations, 
one can get inputs of class C k for arbitrary finite integer k. 
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Remark 5.9. Using the idea presented in Remark 15.81 together with Remark 15.71 it is easy to conclude 
that, for general nilpotent systems, the resulting trajectories are globally C 1 curves and the regularity 
does not depend on the time-parameterization of the trajectories. 

For the sake of completeness, we give in this short appendix the proof of Theorem 11.11 together with 
some comments on Algorithm [2j 



A Proof of Theorem 1.1 



We first note that Steps 1 through 5 in Algorithm |2] are straightforward. 

Theorem 13.51 guarantees that the Desingularization Algorithm (Section 13 .3 j) provides us with a 
new family of vectors fields £ = {£i, . . . , £ m }, which is regular and free up to step r with r denoting 
the maximum value of the degree of nonholonomy of the original system X = {A 1; . . . ,X m }, on the 
corresponding compact set Vj.. Then, we construct the approximate system A^ using the procedure 
presented in Section fl~TI and provide it with the sub-optimal steering law Exact mjr defined in Algorithm 
[5j The sub-optimality of Exact mir is guaranteed by Theorem 15.121 Therefore, by Theorem 14.21 the 
LAS method AppSteer associated with A^ and its steering law is uniformly contractive on the compact 
set V c j. x B R (0). Then, by Theorem EOJ 

GlobalFree (x , Si, e, V c , AppSteer) provided by Algorithm [3] terminates in a finite number of steps and 
stops at a point x such that d(x, x\) < e. Since there is a finite number of compacts to be explored, 
we conclude that Algorithm [2] terminates in a finite number of steps and steers the system (£) from 
^initial £ Q some p i n t x such that d(x, x final ) < e. This ends the proof of Theorem ll.il 



B About the control set 

Let U C M m be any neighborhood of the origin. Then every trajectory of (12.11) corresponding to the 
inputs produced by Algorithm [2] can be time-reparameterized so that the resulting trajectory of (12. ip 
is associated with an input taking values in U. 



C Getting trajectories of class C for the original control sys- 
tem 

We can slightly modify Algorithm [2] to get trajectories of class C 1 for the original control system (£). 
This is equivalent to ask for continuous inputs produced by the algorithm. According to Remark 15.81 
inputs can be made continuous within each iteration step in Algorithm [3] since they are computed based 
the nilpotent approximate system. By using the same interpolation technique as presented in Remark 
15.81 we can still produce inputs which remain continuous from one step to another in Algorithm [3J 
Therefore, trajectories of class C l for the control system (£) are obtained. 
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